//==============================================================================
// Copyright 2018-2020 Kitware, Inc., Kitware SAS
// Author: Guilbert Pierre (Kitware SAS)
//         Cadart Nicolas (Kitware SAS)
// Creation date: 2018-03-27
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//==============================================================================

// This slam algorithm is inspired by the LOAM algorithm:
// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.

// The algorithm is composed of three sequential steps:
//
// - Keypoints extraction: this step consists of extracting keypoints over
// the points clouds. To do that, the laser lines / scans are treated independently.
// The laser lines are projected onto the XY plane and are rescaled depending on
// their vertical angle. Then we compute their curvature and create two classes of
// keypoints. The edges keypoints which correspond to points with a high curvature
// and planar points which correspond to points with a low curvature.
//
// - Ego-Motion: this step consists of recovering the motion of the lidar
// sensor between two frames (two sweeps). The motion is modelized by a constant
// velocity and angular velocity between two frames (i.e null acceleration).
// Hence, we can parameterize the motion by a rotation and translation per sweep / frame
// and interpolate the transformation inside a frame using the timestamp of the points.
// Since the points clouds generated by a lidar are sparse we can't design a
// pairwise match between keypoints of two successive frames. Hence, we decided to use
// a closest-point matching between the keypoints of the current frame
// and the geometric features derived from the keypoints of the previous frame.
// The geometric features are lines or planes and are computed using the edges
// and planar keypoints of the previous frame. Once the matching is done, a keypoint
// of the current frame is matched with a plane / line (depending of the
// nature of the keypoint) from the previous frame. Then, we recover R and T by
// minimizing the function f(R, T) = sum(d(point, line)^2) + sum(d(point, plane)^2).
// Which can be writen f(R, T) = sum((R*X+T-P).t*A*(R*X+T-P)) where:
// - X is a keypoint of the current frame
// - P is a point of the corresponding line / plane
// - A = (n*n.t) with n being the normal of the plane
// - A = (I - n*n.t).t * (I - n*n.t) with n being a director vector of the line
// Since the function f(R, T) is a non-linear mean square error function
// we decided to use the Levenberg-Marquardt algorithm to recover its argmin.
//
// - Localization: This step consists of refining the motion recovered in the Ego-Motion
// step and to add the new frame in the environment map. Thanks to the ego-motion
// recovered at the previous step it is now possible to estimate the new position of
// the sensor in the map. We use this estimation as an initial point (R0, T0) and we
// perform an optimization again using the keypoints of the current frame and the matched
// keypoints of the map (and not only the previous frame this time!). Once the position in the
// map has been refined from the first estimation it is then possible to update the map by
// adding the keypoints of the current frame into the map.
//
// In the following programs, three 3D coordinates system are used :
// - LIDAR {L} : attached to the geometric center of the LiDAR sensor. The
//   coordinates of the received pointclouds are expressed in this system.
//   LIDAR is rigidly linked (static transform) to BASE.
// - BASE  {B} : attached to the origin of the moving body (e.g. vehicle). We
//   are generally interested in tracking an other point of the moving body than
//   the LiDAR's (for example, we prefer to track the GPS antenna pose).
// - WORLD {W} : The world coordinate system {W} coincides with BASE at the
//   initial position. The output trajectory describes BASE origin in WORLD.

// GENERIC
#include <ctime>

// LOCAL
#include "LidarSlam/Utilities.h"
#include "LidarSlam/KDTreePCLAdaptor.h"
#include "LidarSlam/InterpolationModels.h"

// CERES
#include <ceres/solver.h>
// PCL
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>

// EIGEN
#include <Eigen/Dense>

// TEASERPP
#ifdef USE_TEASERPP
#include <pcl/features/normal_3d_omp.h>
#endif //USE_TEASERPP

// LOCAL
// Note: Slam.h needs to be included after pcl/features/xx.h
// because of the conflict between opencv and flann
#include "LidarSlam/Slam.h"

#define PRINT_VERBOSE(minVerbosityLevel, stream) if (this->Verbosity >= (minVerbosityLevel)) {std::cout << stream << std::endl;}
#define IF_VERBOSE(minVerbosityLevel, command) if (this->Verbosity >= (minVerbosityLevel)) { command; }

// Update sensors' parameters
#define ExtSensorMacro(inFuncProto) \
{ \
  if (this->WheelOdomManager) \
    this->WheelOdomManager->inFuncProto; \
  if (this->GravityManager) \
    this->GravityManager->inFuncProto; \
  if (this->ImuManager) \
    this->ImuManager->inFuncProto; \
  for (auto& idLm : this->LandmarksManagers) \
    idLm.second.inFuncProto; \
  if (this->GpsManager) \
    this->GpsManager->inFuncProto; \
  if (this->PoseManager) \
    this->PoseManager->inFuncProto; \
  if (this->CameraManager) \
    this->CameraManager->inFuncProto; \
}

namespace LidarSlam
{

using KDTree = KDTreePCLAdaptor<Slam::Point>;

namespace Utils
{
namespace
{

//-----------------------------------------------------------------------------
//! Approximate pointcloud memory size
inline size_t PointCloudMemorySize(const Slam::PointCloud& cloud)
{
  return (sizeof(cloud) + (sizeof(Slam::PointCloud::PointType) * cloud.size()));
}

} // end of anonymous namespace
} // end of Utils namespace

//==============================================================================
//   Main SLAM use
//==============================================================================

//-----------------------------------------------------------------------------
Slam::Slam()
{
  // Allocate a default Keypoint Extractor for device 0
  this->KeyPointsExtractors[0] = std::make_shared<SpinningSensorKeypointExtractor>();

  // Allocate maps
  for (auto k : this->UsableKeypoints)
    this->LocalMaps[k] = std::make_shared<RollingGrid>();

  // Set default maps parameters
  if (this->UseKeypoints[EDGE])
    this->InitMap(EDGE);
  if (this->UseKeypoints[INTENSITY_EDGE])
    this->InitMap(INTENSITY_EDGE);
  if (this->UseKeypoints[PLANE])
    this->InitMap(PLANE);
  if (this->UseKeypoints[BLOB])
    this->InitMap(BLOB);

  // Reset SLAM internal state
  this->Reset();
}

//-----------------------------------------------------------------------------
void Slam::InitMap(Keypoint k)
{
  // Allocate map
  this->LocalMaps[k] = std::make_shared<RollingGrid>();

  // Set default maps parameters
  this->LocalMaps[k]->SetVoxelResolution(10.);
  this->LocalMaps[k]->SetGridSize(50);

  switch(k)
  {
    case EDGE:
      this->LocalMaps[k]->SetLeafSize(0.3);
      break;
    case INTENSITY_EDGE:
      this->LocalMaps[k]->SetLeafSize(0.3);
      break;
    case PLANE:
      this->LocalMaps[k]->SetLeafSize(0.6);
      break;
    case BLOB:
      this->LocalMaps[k]->SetLeafSize(0.3);
      break;
    default:
      PRINT_ERROR("Unknown keypoint type");
      break;
  }
}

//-----------------------------------------------------------------------------
void Slam::Reset(bool resetLog)
{
  // Reset keypoints maps
  this->ClearLocalMaps();

  // Reset keyframes
  this->KfLastPose = Eigen::Isometry3d::Identity();
  this->KfCounter = 0;

  // n-DoF parameters
  this->Tworld = Eigen::Isometry3d::Identity();
  this->TworldInit = Eigen::Isometry3d::Identity();
  this->Trelative = Eigen::Isometry3d::Identity();

  // Reset pose uncertainty
  this->LocalizationUncertainty = LocalOptimizer::RegistrationError();

  // Reset point clouds
  this->CurrentFrames.clear();
  this->RegisteredFrame.reset(new PointCloud);
  this->CurrentFrames.emplace_back(new PointCloud);
  for (auto k : this->UsableKeypoints)
  {
    this->CurrentRawKeypoints[k].reset(new PointCloud);
    this->CurrentUndistortedKeypoints[k].reset(new PointCloud);
    this->CurrentWorldKeypoints[k].reset(new PointCloud);
  }

  // Reset keypoints matching results
  for (auto k : this->UsableKeypoints)
  {
    this->EgoMotionMatchingResults[k] = KeypointsMatcher::MatchingResults();
    this->LocalizationMatchingResults[k] = KeypointsMatcher::MatchingResults();
  }

  // Reset external sensor managers
  // WARNING : if offline process, measurements should be reloaded from
  // outside this lib. Moreover, landmark managers lost their absolute poses,
  // they would need to be reloaded too.
  this->ResetSensors(true);

  // Reset log history
  if (resetLog)
  {
    // Reset logged keypoints
    this->NbrFrameProcessed = 0;
    this->LogStates.clear();

    // Reset processing duration timers
    Utils::Timer::Reset();
  }

  if (this->IsRecovery())
    this->EndRecovery();
  this->MotionCheck.Reset();
  this->FailDetect.Reset();
  this->RecoveryTimes.clear();
}

//-----------------------------------------------------------------------------
void Slam::StartRecovery(double duration)
{
  PRINT_INFO("LidarSlam : Starting recovery mode")
  this->FailDetect.Reset();
  // Store current parameters
  this->RecoveryStorage = std::make_shared<Recovery::Parameters>();
  this->RecoveryStorage->MapUpdate              = this->MapUpdate;
  this->RecoveryStorage->EgoMotion              = this->EgoMotion;
  this->RecoveryStorage->Undistortion           = this->Undistortion;
  this->RecoveryStorage->ICPMaxIter             = this->GetLocalizationICPMaxIter();
  this->RecoveryStorage->MaxNeighborsDistance   = this->GetLocalizationMaxNeighborsDistance();
  this->RecoveryStorage->InitSaturationDistance = this->GetLocalizationInitSaturationDistance();

  // Set recovery parameters (not real time,
  // some frames might be dropped)
  LidarSlam::Recovery::Parameters recovery;
  this->SetEgoMotion(recovery.EgoMotion);
  this->SetUndistortion(recovery.Undistortion);
  this->SetMapUpdate(recovery.MapUpdate);
  this->SetLocalizationICPMaxIter(recovery.ICPMaxIter);
  this->SetLocalizationMaxNeighborsDistance(recovery.MaxNeighborsDistance);
  this->SetLocalizationInitSaturationDistance(recovery.InitSaturationDistance);

  // Recover previous trajectory
  while (std::abs(this->LogStates.back().Time - this->CurrentTime) < duration)
  {
    if (this->LogStates.empty())
      break;
    this->LogStates.pop_back();
  }

  // Recompute the maps with inlier poses
  this->UpdateMaps();

  // Store timestamp to handle trajectory after recovery
  this->RecoveryTimes.push_back(this->LogStates.back().Time);

  // Next frames will be processed in recovery mode
}

//-----------------------------------------------------------------------------
void Slam::EndRecovery()
{
  PRINT_INFO("LidarSlam : Ending recovery mode")
  this->FailDetect.Reset();
  // Reset parameters
  this->SetEgoMotion(this->RecoveryStorage->EgoMotion);
  this->SetUndistortion(this->RecoveryStorage->Undistortion);
  this->SetMapUpdate(this->RecoveryStorage->MapUpdate);
  LidarSlam::Recovery::Parameters recovery;
  if (this->GetLocalizationICPMaxIter() != recovery.ICPMaxIter)
    PRINT_WARNING("LidarSlam : ICP number of iterations is reset to " << this->RecoveryStorage->ICPMaxIter)
  this->SetLocalizationICPMaxIter(this->RecoveryStorage->ICPMaxIter);
  if (this->GetLocalizationMaxNeighborsDistance() != recovery.MaxNeighborsDistance)
    PRINT_WARNING("LidarSlam : Maximum distance between neighbors is reset to " << this->RecoveryStorage->MaxNeighborsDistance)
  this->SetLocalizationMaxNeighborsDistance(this->RecoveryStorage->MaxNeighborsDistance);
  if (std::abs(this->GetLocalizationInitSaturationDistance() - recovery.InitSaturationDistance) > 1e-6)
    PRINT_WARNING("LidarSlam : Initial saturation distance is reset to " << this->RecoveryStorage->InitSaturationDistance)
  this->SetLocalizationInitSaturationDistance(this->RecoveryStorage->InitSaturationDistance);

  // Free memory of param storage
  this->RecoveryStorage.reset();
}

//-----------------------------------------------------------------------------
void Slam::SetNbThreads(int n)
{
  // Set number of threads for main processes
  this->NbThreads = n;
  // Set number of threads for keypoints extraction
  for (const auto& kv : this->KeyPointsExtractors)
    kv.second->SetNbThreads(n);
}

//-----------------------------------------------------------------------------
void Slam::SetVerbosity(int verbosity)
{
  this->Verbosity = verbosity;
  this->MotionCheck.SetVerbose(verbosity >= 3);
  ExtSensorMacro(SetVerbose(verbosity >= 3));
}

//-----------------------------------------------------------------------------
void Slam::SetInterpolation(Interpolation::Model model)
{
  this->Interpolation = model;
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetInterpolationModel(model);
  if (this->PoseManager)
    this->PoseManager->SetInterpolationModel(model);
  if (this->ImuManager)
    this->ImuManager->SetInterpolationModel(model);
}

//-----------------------------------------------------------------------------
void Slam::EnableKeypointType(Keypoint k, bool enabled)
{
  this->UseKeypoints[k] = enabled;
  if (enabled)
  {
    if (!this->LocalMaps.count(k))
      this->InitMap(k);
    if (!this->CurrentRawKeypoints.count(k))
      this->CurrentRawKeypoints[k].reset(new PointCloud);
    if (!this->CurrentUndistortedKeypoints.count(k))
      this->CurrentUndistortedKeypoints[k].reset(new PointCloud);
    if (!this->CurrentWorldKeypoints.count(k))
      this->CurrentWorldKeypoints[k].reset(new PointCloud);
    this->EgoMotionMatchingResults[k] = KeypointsMatcher::MatchingResults();
    this->LocalizationMatchingResults[k] = KeypointsMatcher::MatchingResults();
  }
}

//-----------------------------------------------------------------------------
bool Slam::KeypointTypeEnabled(Keypoint k) const
{
  if (!this->UseKeypoints.count(k))
    return false;
  return this->UseKeypoints.at(k);
}

//-----------------------------------------------------------------------------
bool Slam::IsExtSensorForLocalOpt()
{
  return (this->WheelOdomManager && this->WheelOdomManager->CanBeUsedLocally()) ||
         (this->GravityManager && this->GravityManager->CanBeUsedLocally()) ||
         (this->ImuManager && this->ImuManager->CanBeUsedLocally()) ||
          this->LmCanBeUsedLocally() ||
         (this->PoseManager && this->PoseManager->CanBeUsedLocally()) ||
         this->CameraCanBeUsedLocally();
}

//-----------------------------------------------------------------------------
void Slam::AddFrames(const std::vector<PointCloud::Ptr>& frames)
{
  Utils::Timer::Init("SLAM frame processing");

  // Check that input frames are correct and can be processed
  if (!this->CheckFrames(frames))
    return;
  this->CurrentFrames = frames;
  this->CurrentTime = Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);

  // Set init pose (can have been modified by global optimization / reset)
  // 1) To ensure a smooth local SLAM, the global optimization must refine
  // poses relatively to last pose, i.e, last pose must be fixed.
  // 2) To ensure a fix reference point, the global optimization must refine
  // poses relatively to starting point, i.e, last pose can have changed.
  // If LogStates empty, do not modify Tworld (must be identity after Reset)
  if (!this->LogStates.empty())
    this->Tworld = this->LogStates.back().Isometry;
  else
    this->Tworld = this->TworldInit;

  // Create UsableKeypointTypes for new frame
  // The keypoints cannot be chosen while processing a frame
  // because it impacts all the maps structure along the process
  this->UsableKeypoints.clear();
  for (auto k : KeypointTypes)
  {
    if (this->UseKeypoints[k])
      this->UsableKeypoints.push_back(k);
  }

  PRINT_VERBOSE(2, "\n#########################################################");
  PRINT_VERBOSE(1, "Processing frame " << this->NbrFrameProcessed << std::fixed << std::setprecision(9) <<
                   " (at time " << this->CurrentTime << ")" << std::scientific);
  PRINT_VERBOSE(2, "#########################################################\n");

  // Compute the edge and planar keypoints
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints extraction"));
  this->ExtractKeypoints();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints extraction"));

  // Estimate Trelative by extrapolating new pose with a constant velocity model
  // and/or registering current frame on previous one
  IF_VERBOSE(3, Utils::Timer::Init("Ego-Motion"));
  this->ComputeEgoMotion();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Ego-Motion"));

  if (this->IsExtSensorForLocalOpt())
  {
    IF_VERBOSE(3, Utils::Timer::Init("External sensor constraints computation"));
    this->ComputeSensorConstraints();
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("External sensor constraints computation"));
  }

  // Perform Localization : update Tworld from map and current frame keypoints
  // and optionally undistort keypoints clouds based on ego-motion
  IF_VERBOSE(3, Utils::Timer::Init("Localization"));
  this->Localization();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization"));

  // Update the tags if requested
  if (this->LandmarkConstraintLocal)
  {
    for (auto& idLm : this->LandmarksManagers)
    {
      if (idLm.second.UpdateAbsolutePose(this->Tworld, this->CurrentTime))
        PRINT_VERBOSE(3, "Updating reference pose of tag #" << idLm.first << " to "<<idLm.second.GetAbsolutePose().transpose());
    }
  }

  // Compute and check pose confidence estimators
  // Must be set before maps update because the overlap computation
  // requires the current KdTree. This KdTree is reset in the maps update.
  IF_VERBOSE(3, Utils::Timer::Init("Confidence estimators computation"));
  this->EstimateOverlap();
  this->CheckMotionLimits();

  // If in recovery mode stop the process here
  if (this->IsRecovery())
    return;

  // Check if the map is starting
  unsigned int nbMapKpts = 0;
  for (const auto& mapKptsCloud : this->LocalMaps)
    nbMapKpts += mapKptsCloud.second->Size();
  bool startingMap = nbMapKpts < this->MinNbMatchedKeypoints * 10;

  if (this->FailureDetectionEnabled)
  {
    this->FailDetect.AddConfidence(this->OverlapEstimation,
                                   this->GetPositionErrorStd(),
                                   this->ComplyMotionLimits,
                                   startingMap || this->OptimizationValid,
                                   this->CurrentTime);
  }

  if (this->FailDetect.HasFailed())
  {
    PRINT_ERROR("SLAM has failed")
    ++this->NbrFrameProcessed;
    return;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Confidence estimators computation"));

  // A keyframe is a frame that has moved enough
  // or that is required by the tag manager and for
  // which the optimization has been performed correctly
  // (or exceptionnally when the map is initializing)
  this->IsKeyFrame = (this->OptimizationValid || startingMap ) && this->NeedNewKeyFrame();
  if (this->IsKeyFrame)
  {
    // Notify current frame to be a new keyframe
    this->KfCounter++;
    this->KfLastPose = this->Tworld;

    // Update keypoints maps if required: add current keypoints to map using Tworld
    // If mapping mode is ADD_KPTS_TO_FIXED_MAP the initial map points will remain untouched
    // If mapping mode is UPDATE, the initial map points can disappear.
    if (this->MapUpdate == MappingMode::ADD_KPTS_TO_FIXED_MAP ||
        this->MapUpdate == MappingMode::UPDATE)
    {
      IF_VERBOSE(3, Utils::Timer::Init("Maps update"));
      this->UpdateMapsUsingTworld();
      IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Maps update"));
    }
  }

  // Log current frame processing results : pose, covariance and keypoints.
  IF_VERBOSE(3, Utils::Timer::Init("Logging"));
  this->LogCurrentFrameState();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Logging"));

  if (this->OptimizationValid &&
      this->ImuUpdate &&
      this->ImuHasData())
  {
    // Update IMU graph with new slam pose to refine the biases
    IF_VERBOSE(3, Utils::Timer::Init("IMU biases refinement"));
    if (this->ImuManager->Update(this->LogStates.back()))
      ++this->ImuHasBeenUpdated;
    else
      this->ImuHasBeenUpdated = 0;
    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("IMU biases refinement"));
  }

  // Motion and localization parameters estimation information display
  if (this->Verbosity >= 2)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "========== SLAM results ==========\n";
    std::cout << "Ego-Motion:\n"
                 " translation = [" << this->Trelative.translation().transpose()                                        << "] m\n"
                 " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Trelative.linear())).transpose() << "] °\n"
                 "Localization:\n"
                 " position    = [" << this->Tworld.translation().transpose()                                        << "] m\n"
                 " orientation = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Tworld.linear())).transpose() << "] °" << std::endl;
    RESET_COUT_FIXED_PRECISION;
  }

  if (this->Verbosity >= 5)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "========== Memory usage ==========\n";
    std::map<Keypoint, unsigned int> points;
    std::map<Keypoint, unsigned int> memory;
    // Initialize number of points and memory per keypoint type
    for (auto k : this->UsableKeypoints)
    {
      points[k] = 0;
      memory[k] = 0;
    }
    // Sum points and memory allocated of each keypoints cloud
    for (auto const& st: this->LogStates)
    {
      for (auto k : this->UsableKeypoints)
      {
        points[k] += st.Keypoints.at(k)->PointsSize();
        memory[k] += st.Keypoints.at(k)->MemorySize();
      }
    }

    // Print keypoints memory usage
    for (auto k : this->UsableKeypoints)
    {
      std::cout << Utils::Capitalize(Utils::Plural(KeypointTypeNames.at(k)))<< " log  : "
                << LogStates.size() << " frames, "
                << points[k] * 1e-6 << " points, "
                << memory[k] << " MB\n";

    }
    RESET_COUT_FIXED_PRECISION;
  }

  // Frame processing duration
  this->NbrFrameProcessed++;
  IF_VERBOSE(1, Utils::Timer::StopAndDisplay("SLAM frame processing"));
}

//-----------------------------------------------------------------------------
void Slam::ComputeSensorConstraints()
{
  if (this->WheelOdomManager && this->WheelOdomManager->CanBeUsedLocally() &&
      this->WheelOdomManager->ComputeConstraint(this->CurrentTime))
    PRINT_VERBOSE(3, "Wheel odometry constraint added")
  if (this->GravityManager && this->GravityManager->CanBeUsedLocally() &&
      this->GravityManager->ComputeConstraint(this->CurrentTime))
    PRINT_VERBOSE(3, "IMU gravity constraint added")
  if (this->LmCanBeUsedLocally())
  {
    for (auto& idLm : this->LandmarksManagers)
    {
      PRINT_VERBOSE(3, "Checking state of tag #" << idLm.first)
      if (idLm.second.ComputeConstraint(this->CurrentTime))
        PRINT_VERBOSE(3, "\t Adding constraint for tag #" << idLm.first)
    }
  }
  if (this->PoseManager && this->PoseManager->CanBeUsedLocally())
  {
    if (!this->LogStates.empty())
    {
      // Update last pose information because the external pose constraint is relative
      // The last pose logged corresponds to last lidar frame
      this->PoseManager->SetPrevLidarTime(this->LogStates.back().Time);
      this->PoseManager->SetPrevPoseTransform(this->LogStates.back().Isometry);
      if (this->PoseManager->ComputeConstraint(this->CurrentTime))
        PRINT_VERBOSE(3, "External pose constraint added")
    }
  }
  if (this->CameraCanBeUsedLocally())
  {
    if (!this->LogStates.empty())
    {
      // Update last pose information because the camera constraint is relative
      // The last pose logged corresponds to last lidar frame
      this->CameraManager->SetPrevLidarTime(this->LogStates.back().Time);
      this->CameraManager->SetPrevPoseTransform(this->LogStates.back().Isometry);
      if (this->CameraManager->ComputeConstraint(this->CurrentTime))
        PRINT_VERBOSE(3, "Camera constraint added")
    }
    // Store the current frame for next iteration
    PointCloud::Ptr aggregatedFrames = this->AggregateFrames(this->CurrentFrames);
    this->CameraManager->SetPrevLidarFrame(aggregatedFrames);
  }
}

//-----------------------------------------------------------------------------
void Slam::UpdateMaps(bool resetMaps)
{
  if(resetMaps)
    this->ClearLocalMaps();
  else
  {
    // Remove points older than the first logged state
    for (auto k : this->UsableKeypoints)
      this->LocalMaps[k]->ClearPoints(this->LogStates.front().Time, false);
  }
  // Update LocalMaps with the keypoints stored in the LogStates
  this->BuildMaps(this->LocalMaps);
}

//-----------------------------------------------------------------------------
bool Slam::UpdateTrajectoryAndMapsWithIMU()
{
  #ifdef USE_GTSAM
  if (this->ImuHasData())
  {
    IF_VERBOSE(1, Utils::Timer::Init("Pose graph optimization"));
    // Update LogStates and maps
    // Reset maps
    this->ClearLocalMaps();

    // Update LogStates and maps
    for (auto& state : this->LogStates)
    {
      if (state.Index < 1 || !state.IsKeyFrame)
        continue;
      std::cout << "Updating state #" << state.Index << std::endl;
      // Update Isometry
      ExternalSensors::PoseMeasurement synchMeas;
      this->PoseManager->ComputeSynchronizedMeasureBase(state.Time, synchMeas);
      state.Isometry = synchMeas.Pose;
      // Update keypoints and maps
      for (auto k : this->UsableKeypoints)
      {
        // Get keypoints
        PointCloud::Ptr rawKeypoints = state.RawKeypoints[k]->GetCloud();
        PointCloud::Ptr undistortedKeypoints(new PointCloud);
        *undistortedKeypoints = *rawKeypoints;

        // Update undistorted keypoints
        this->UndistortWithPoseMeasurement(undistortedKeypoints, state.Time);
        state.Keypoints[k]->SetCloud(undistortedKeypoints, this->LoggingStorage);
        // Update maps
        PointCloud::Ptr keypoints(new PointCloud);
        keypoints->header = Utils::BuildPclHeader(state.Time, this->BaseFrameId, state.Index);
        pcl::transformPointCloud(*undistortedKeypoints, *keypoints, state.Isometry.matrix().cast<float>());
        this->LocalMaps[k]->Add(keypoints, false);
      }
    }
    this->Tworld = this->LogStates.back().Isometry;

    IF_VERBOSE(1, Utils::Timer::StopAndDisplay("Pose graph optimization"));
    return true;
  }
  PRINT_ERROR("No IMU data found, trajectory is not optimized")
  return false;
  #else
  PRINT_ERROR("GTSAM was not found, IMU cannot be used, the trajectory is not optimized")
  return false;
  #endif
}

//-----------------------------------------------------------------------------
bool Slam::OptimizeGraph()
{
  #ifdef USE_G2O
  // Check if graph can be optimized
  if (!this->LmHasData() && !this->GpsHasData() && !this->UsePGOConstraints[LOOP_CLOSURE])
  {
    PRINT_WARNING("No external constraint found, graph cannot be optimized");
    return false;
  }

  PoseGraphOptimizer graphManager;
  graphManager.SetFixFirst(this->FixFirstVertex);
  graphManager.SetFixLast(this->FixLastVertex);
  // Clear the graph
  graphManager.ResetGraph();
  // Init pose graph optimizer
  graphManager.SetNbIteration(this->NbGraphIterations);
  graphManager.SetVerbose(this->Verbosity >= 2);
  graphManager.SetSaveG2OFile(!this->G2oFileName.empty());
  graphManager.SetG2OFileName(this->G2oFileName);
  // Add new SLAM states to graph
  graphManager.AddLidarStates(this->LogStates);

  IF_VERBOSE(1, Utils::Timer::Init("Pose graph optimization"));
  IF_VERBOSE(3, Utils::Timer::Init("PGO : optimization"));

  // Boolean to store the info "there is at least one external constraint in the graph"
  bool externalConstraint = false;

  // Look for loop closure constraints
  if (this->UsePGOConstraints[LOOP_CLOSURE])
  {
    // Detect loop closure
    auto itRevisitedState = this->LogStates.begin();
    auto itQueryState = itRevisitedState;
    if (DetectLoopClosureIndices(itQueryState, itRevisitedState))
    {
      // Compute a loopClosureTransform from the revisited frame to the query frame
      // by registering the keypoints of the query frame onto the keypoints of the revisited frame
      Eigen::Isometry3d loopClosureTransform;
      Eigen::Matrix6d loopClosureCovariance;
      if (this->LoopClosureRegistration(itQueryState, itRevisitedState,
                                        loopClosureTransform, loopClosureCovariance))
      {
        // Add loop closure constraint into pose graph
        graphManager.AddLoopClosureConstraint(this->LoopParams.QueryIdx, this->LoopParams.RevisitedIdx,
                                              loopClosureTransform, loopClosureCovariance);
        externalConstraint = true;
      }
    }
    else
      PRINT_WARNING("No loop closure is detected for pose graph optimization.")
  }

  // Look for landmark constraints
  if (this->UsePGOConstraints[LANDMARK] && this->LmHasData())
  {
    // Allow the rotation of the covariances when interpolating the measurements
    this->SetLandmarkCovarianceRotation(true);
    // Set the landmark detector calibration
    graphManager.AddExternalSensor(this->LmDetectorCalibration, ExternalSensor::LANDMARK_DETECTOR);

    for (auto& idLm : this->LandmarksManagers)
    {
      // Shortcut to current manager
      auto& lm = idLm.second;

      // Add landmark to the graph
      Eigen::Vector6d lmPose = lm.GetAbsolutePose();
      Eigen::Isometry3d lmTransfo = Utils::XYZRPYtoIsometry(lmPose.data());
      graphManager.AddLandmark(lmTransfo, idLm.first, lm.GetPositionOnly());

      // Add landmarks constraint to the graph
      lm.SetVerbose(false);
      for (auto& s : this->LogStates)
      {
        ExternalSensors::LandmarkMeasurement lmSynchMeasure; // Virtual landmark measure with synchronized timestamp and no calibration applied
        if (!lm.ComputeSynchronizedMeasure(s.Time, lmSynchMeasure))
          continue;
        // Add synchronized landmark observations to the graph
        graphManager.AddLandmarkConstraint(s.Index, idLm.first, lmSynchMeasure, lm.GetPositionOnly());
        externalConstraint = true;
      }
      lm.SetVerbose(this->Verbosity >= 3);
    }
    // Reset the rotate covariance member to not rotate covariances
    // in future local constraints building
    this->SetLandmarkCovarianceRotation(false);
  }

  // Look for GPS constraints
  if (this->UsePGOConstraints[PGO_GPS] && this->GpsHasData())
  {
    graphManager.AddExternalSensor(this->GpsManager->GetCalibration(), ExternalSensor::GPS);
    for (auto& s : this->LogStates)
    {
      ExternalSensors::GpsMeasurement gpsSynchMeasure; // Virtual GPS measure in SLAM reference frame with synchronized timestamp
      if (!this->GpsManager->ComputeSynchronizedMeasureOffset(s.Time, gpsSynchMeasure))
        continue;

      // Add synchronized gps measurement to the graph
      graphManager.AddGpsConstraint(s.Index, gpsSynchMeasure);
      externalConstraint = true;
    }
  }

  if (!externalConstraint)
  {
    PRINT_ERROR("No external constraints nor loop closure constraint exist. Pose graph can not be optimized");
    return false;
  }

  auto statesInit = this->LogStates;

  // Run pose graph optimization
  if (!graphManager.Process(this->LogStates))
  {
    PRINT_ERROR("Pose graph optimization failed.");
    return false;
  }

  // WARNING : covariances are not updated at each graph optimization
  // because g2o does not allow to reach them.
  // Covariances rotation is mandatory if covariances are to be used again afterwards
  auto itStates = this->LogStates.begin();
  auto itInit = statesInit.begin();
  while (itInit != statesInit.end())
  {
    // Compute relative transform
    Eigen::Isometry3d Trel = itInit->Isometry.inverse() * itStates->Isometry;
    Eigen::Vector6d pose = Utils::IsometryToXYZRPY(itInit->Isometry);
    CeresTools::RotateCovariance(pose, itStates->Covariance, Trel); // new = init * Trel
    ++itStates;
    ++itInit;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : optimization"));

  // Update the maps from the beginning using the new trajectory
  IF_VERBOSE(3, Utils::Timer::Init("PGO : maps update"));
  this->UpdateMaps();
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("PGO : maps update"));

  // The last pose has to be updated with new optimized pose
  this->SetWorldTransformFromGuess(this->LogStates.back().Isometry);

  IF_VERBOSE(1, Utils::Timer::StopAndDisplay("Pose graph optimization"));

  return true;

  #else
  PRINT_ERROR("SLAM graph optimization requires G2O, but it was not found.");
  return false;
  #endif  // USE_G2O
}

//-----------------------------------------------------------------------------
void Slam::EnablePGOConstraint(PGOConstraint constraint, bool enabled)
{
  this->UsePGOConstraints[constraint] = enabled;
}

//-----------------------------------------------------------------------------
bool Slam::IsPGOConstraintEnabled(PGOConstraint constraint) const
{
  if (!this->UsePGOConstraints.count(constraint))
    return false;
  return this->UsePGOConstraints.at(constraint);
}

//-----------------------------------------------------------------------------
void Slam::SetWorldTransformFromGuess(const Eigen::Isometry3d& poseGuess)
{
  // Store pose in case of reinitialization need
  this->TworldInit = poseGuess;
  // Set current pose
  this->Tworld = poseGuess;

  // Ego-Motion estimation is not valid anymore since we imposed a discontinuity.
  // We reset previous pose so that previous ego-motion extrapolation results in Identity matrix.
  // We reset current frame keypoints so that ego-motion registration will be skipped for next frame.
  if (!this->LogStates.empty())
    this->LogStates.back().Isometry = this->Tworld;
  for (auto k : this->UsableKeypoints)
    this->CurrentRawKeypoints[k].reset(new PointCloud);
  if (this->ImuManager)
    this->ImuManager->SetInitBasePose(this->Tworld);
}

//-----------------------------------------------------------------------------
void Slam::SaveMapsToPCD(const std::string& filePrefix, PCDFormat pcdFormat, bool filtered)
{
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints maps saving to PCD"));

  // Rebuild LocalMaps when the time threshold is set to remove old points
  // so that the removed points are recovered in LocalMaps.
  // Check LoggingTimeout is greater than DecayingThreshold to make sure there are enough keyframes stored in Logstates
  if ( this->LoggingTimeout > this->GetVoxelGridDecayingThreshold() && this->GetVoxelGridDecayingThreshold() > 0)
    this->UpdateMaps(true);

  // Save keypoint maps
  for (auto k : this->UsableKeypoints)
    savePointCloudToPCD(filePrefix + "_" + Utils::Plural(KeypointTypeNames.at(k)) + ".pcd",  *this->GetMap(k, filtered),  pcdFormat, true);

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints maps saving to PCD"));
}

//-----------------------------------------------------------------------------
void Slam::LoadMapsFromPCD(const std::string& filePrefix, bool resetMaps)
{
  IF_VERBOSE(3, Utils::Timer::Init("Keypoints maps loading from PCD"));

  // In most of the cases, we would like to reset SLAM internal maps before
  // loading new maps to avoid conflicts.
  if (resetMaps)
    this->ClearLocalMaps();

  for (auto k : this->UsableKeypoints)
  {
    std::string path = filePrefix + Utils::Plural(KeypointTypeNames.at(k)) + ".pcd";
    PointCloud::Ptr keypoints(new PointCloud);
    if (pcl::io::loadPCDFile(path, *keypoints) == 0)
    {
      std::cout << "SLAM keypoints map successfully loaded from " << path << std::endl;
      // If mapping mode is NONE or ADD_DECAYING_KPTS, the first map points are fixed,
      // else, the initial map points can be updated
      bool fixedMap = this->MapUpdate == MappingMode::NONE || this->MapUpdate == MappingMode::ADD_KPTS_TO_FIXED_MAP;
      this->LocalMaps[k]->Add(keypoints, fixedMap);
    }
  }
  // TODO : load/use map origin (in which coordinates?) in title or VIEWPOINT field
  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Keypoints maps loading from PCD"));
}

//-----------------------------------------------------------------------------
void Slam::ResetStatePoses(ExternalSensors::PoseManager& newTrajectoryManager)
{
  double startTime = newTrajectoryManager.GetMeasures().front().Time;
  double endTime   = newTrajectoryManager.GetMeasures().back().Time;
  if (startTime > this->LogStates.back().Time || endTime < this->LogStates.front().Time)
  {
    PRINT_WARNING("Unable to reset poses with new trajectory : timestamps are different from lidar time.");
    return;
  }

  auto itState = this->LogStates.begin();
  // Get iterator pointing to the first measurement after new trajectory time
  while (itState->Time < startTime)
    ++itState;

  // Save the state before endTime of new trajectory
  // when new trajectory is shorter than Logstates
  Eigen::Isometry3d endTimeState = Eigen::Isometry3d::Identity();
  auto itEndTimeState = itState;
  if (endTime < this->LogStates.back().Time)
  {
    while (itEndTimeState->Time <= endTime)
      ++itEndTimeState;
    --itEndTimeState;
    endTimeState = itEndTimeState->Isometry;
  }

  // Virtual measure with synchronized timestamp
  ExternalSensors::PoseMeasurement synchMeas;
  while (itState->Time <= endTime && itState != this->LogStates.end())
  {
    newTrajectoryManager.ComputeSynchronizedMeasureBase(itState->Time, synchMeas);
    itState->Isometry = synchMeas.Pose;
    itState->Covariance = synchMeas.Covariance;
    ++itState;
  }

  // If new trajectory is shorter than Logstates, update poses after endTime
  if (endTime < this->LogStates.back().Time)
  {
    while (itState != this->LogStates.end())
    {
      Eigen::Isometry3d tRelative = endTimeState.inverse() * itState->Isometry;
      endTimeState = itState->Isometry;
      itState->Isometry = itEndTimeState->Isometry * tRelative;
      ++itEndTimeState;
      ++itState;
    }
  }

  // Update LocalMaps with new poses
  this->UpdateMaps();
}

//==============================================================================
//   SLAM results getters
//==============================================================================

//-----------------------------------------------------------------------------
std::unordered_map<std::string, double> Slam::GetDebugInformation() const
{
  std::unordered_map<std::string, double> map;
  for (Keypoint k : this->UsableKeypoints)
  {
    std::string nameEgo = "EgoMotion: " + Utils::Plural(KeypointTypeNames.at(k)) + " used";
    map[nameEgo] = this->EgoMotionMatchingResults.at(k).NbMatches();
    std::string nameLoc = "Localization: " + Utils::Plural(KeypointTypeNames.at(k)) + " used";
    map[nameLoc] = this->LocalizationMatchingResults.at(k).NbMatches();
  }

  map["Confidence: position error"]       = this->LocalizationUncertainty.PositionError;
  map["Confidence: orientation error"]    = this->LocalizationUncertainty.OrientationError;
  map["Confidence: overlap"]              = this->OverlapEstimation;
  map["Confidence: comply motion limits"] = this->ComplyMotionLimits;

  return map;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, std::vector<double>> Slam::GetDebugArray() const
{
  auto toDoubleVector = [](auto const& scalars) { return std::vector<double>(scalars.begin(), scalars.end()); };

  std::unordered_map<std::string, std::vector<double>> map;
  for (auto k : this->UsableKeypoints)
  {
    std::string name = "EgoMotion: " + KeypointTypeNames.at(k) + " matches";
    map[name]  = toDoubleVector(this->EgoMotionMatchingResults.at(k).Rejections);
    name = "EgoMotion: " + KeypointTypeNames.at(k) + " weights";
    map[name]  = this->EgoMotionMatchingResults.at(k).Weights;
  }

  for (auto k : this->UsableKeypoints)
  {
    std::string name = "Localization: " + KeypointTypeNames.at(k) + " matches";
    map[name]  = toDoubleVector(this->LocalizationMatchingResults.at(k).Rejections);
    name = "Localization: " + KeypointTypeNames.at(k) + " weights";
    map[name]  = this->LocalizationMatchingResults.at(k).Weights;
  }

  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetRegisteredFrame()
{
  // If the input points have not been aggregated to WORLD coordinates yet,
  // transform and aggregate them
  if (this->RegisteredFrame->header.stamp != this->CurrentFrames[0]->header.stamp)
    this->RegisteredFrame = this->AggregateFrames(this->CurrentFrames, true, true);
  return this->RegisteredFrame;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetMap(Keypoint k, bool clean) const
{
  PointCloud::Ptr map = this->LocalMaps.at(k)->Get(clean);
  map->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                      this->WorldFrameId,
                                      this->NbrFrameProcessed);
  return map;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetTargetSubMap(Keypoint k) const
{
  PointCloud::Ptr subMap = this->LocalMaps.at(k)->GetSubMap();
  subMap->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                         this->WorldFrameId,
                                         this->NbrFrameProcessed);
  return subMap;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::GetKeypoints(Keypoint k, bool worldCoordinates)
{
  // Return keypoints in BASE coordinates
  if (!worldCoordinates)
    return this->CurrentUndistortedKeypoints.at(k);

  // Return keypoints in WORLD coordinates
  // If the keypoints have not been transformed yet to WORLD coordinates, perform transformation
  if (this->CurrentWorldKeypoints.at(k)->header.stamp != this->CurrentUndistortedKeypoints.at(k)->header.stamp)
    this->CurrentWorldKeypoints[k] = this->TransformPointCloud(this->CurrentUndistortedKeypoints[k],
                                                               this->Tworld, this->WorldFrameId);
  return this->CurrentWorldKeypoints.at(k);
}

//==============================================================================
//   Main SLAM steps
//==============================================================================

//-----------------------------------------------------------------------------
bool Slam::CheckFrames(const std::vector<PointCloud::Ptr>& frames)
{
  // Check input frames and return if they are all empty
  bool allFramesEmpty = true;
  for (unsigned int i = 0; i < frames.size(); ++i)
  {
    if (!frames[i] || !frames[i]->empty())
      allFramesEmpty = false;
    else
      PRINT_WARNING("SLAM input frame " << i << " is an empty pointcloud : frame ignored.");
  }
  if (allFramesEmpty)
  {
    PRINT_ERROR("SLAM input only contains empty pointclouds : exiting.");
    return false;
  }

  // Skip frames if it has the same timestamp as previous ones (will induce problems in extrapolation)
  if (frames[0]->header.stamp == this->CurrentFrames[0]->header.stamp)
  {
    PRINT_ERROR("SLAM frames have the same timestamp (" << frames[0]->header.stamp << ") as previous ones : frames ignored.");
    return false;
  }

  // Check frame dropping
  for (unsigned int i = 0; i < frames.size(); ++i)
  {
    unsigned int droppedFrames = frames[i]->header.seq - this->PreviousFramesSeq[i] - 1;
    if ((this->PreviousFramesSeq[i] > 0) && (droppedFrames > 0))
      PRINT_WARNING(droppedFrames << " frame(s)" << (frames.size() > 1 ? " from LiDAR device " + std::to_string(i) : "") << " were dropped by SLAM\n");
    this->PreviousFramesSeq[i] = frames[i]->header.seq;
  }

  return true;
}

//-----------------------------------------------------------------------------
void Slam::ExtractKeypoints()
{
  PRINT_VERBOSE(2, "========== Keypoints extraction ==========");

  // Current keypoints become previous ones
  this->PreviousRawKeypoints = this->CurrentRawKeypoints;

  // Extract keypoints from each input cloud
  std::map<Keypoint, std::vector<PointCloud::Ptr>> keypoints;
  for (const auto& frame: this->CurrentFrames)
  {
    // If the frame is empty, ignore it
    if (frame->empty())
      continue;

    // Get keypoints extractor to use for this LiDAR device
    int lidarDevice = frame->front().device_id;
    // Check if KE exists
    if (!this->KeyPointsExtractors.count(lidarDevice))
    {
      // If KE does not exist but we are only using a single KE, use default one
      if (this->KeyPointsExtractors.size() == 1)
      {
        PRINT_WARNING("Input frame comes from LiDAR device " << lidarDevice
                    << " but no keypoints extractor has been set for this device : using default extractor for device 0.");
        lidarDevice = 0;
      }
      // Otherwise ignore frame
      else
      {
        PRINT_ERROR("Input frame comes from LiDAR device " << lidarDevice
                    << " but no keypoints extractor has been set for this device : ignoring frame.");
        continue;
      }
    }
    KeypointExtractorPtr& ke = this->KeyPointsExtractors[lidarDevice];
    ke->Enable(this->UsableKeypoints);
    // Extract keypoints from this frame
    ke->ComputeKeyPoints(frame);
    for (auto k : this->UsableKeypoints)
      keypoints[k].push_back(ke->GetKeypoints(k));
  }

  // Merge all keypoints extracted from different frames together
  for (auto k : this->UsableKeypoints)
    this->CurrentRawKeypoints[k] = this->AggregateFrames(keypoints[k], false, false);

  if (this->Verbosity >= 2)
  {
    std::cout << "Extracted features : ";
    for (auto k : this->UsableKeypoints)
      std::cout << this->CurrentRawKeypoints[k]->size() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    std::cout << std::endl;
  }
}

//-----------------------------------------------------------------------------
bool Slam::InitTworldWithPoseMeasurement(double time)
{
  // Compute synchronized pose
  if (!this->PoseManager)
  {
    PRINT_WARNING("No external pose manager : Lidar pose not initialized")
    return false;
  }

  if (time < 0 && this->LogStates.empty())
  {
    PRINT_WARNING("No indicated time and LogStates is empty : Lidar pose not initialized")
    return false;
  }

  if (this->LogStates.empty())
  {
    // If LogStates is empty, use directly external pose to init Tworld
    ExternalSensors::PoseMeasurement synchMeas; // Virtual measure with synchronized timestamp and calibration applied
    if (!this->PoseManager->ComputeSynchronizedMeasureBase(time, synchMeas))
    {
      PRINT_WARNING("Cannot find synchronized pose measurement : Lidar pose not initialized")
      return false;
    }
    this->TworldInit = synchMeas.Pose;
    this->Tworld = synchMeas.Pose;
  }
  else
  {
    // If LogStates is not empty, find the closest state to time in LogStates
    // Update this state with the external pose at this time
    // Update LogStates and maps relatively
    auto itSt = time < 0? std::prev(this->LogStates.end()) : this->GetClosestState(time);

    // Compute the external pose at the time of the found state
    ExternalSensors::PoseMeasurement synchMeas; // Virtual measure with synchronized timestamp and calibration applied
    if (!this->PoseManager->ComputeSynchronizedMeasureBase(itSt->Time, synchMeas))
    {
      PRINT_WARNING("Cannot find synchronized pose measurement : Lidar pose not initialized")
      return false;
    }

    Eigen::Isometry3d update = synchMeas.Pose * itSt->Isometry.inverse();
    for (auto& state: this->LogStates)
    {
      // Rotate covariance
      Eigen::Vector6d initPose = Utils::IsometryToXYZRPY(state.Isometry);
      CeresTools::RotateCovariance(initPose, state.Covariance, update, true); // new = update * init
      // Update isometry
      state.Isometry = update * state.Isometry;
    }

    // Update Tworld and TworldInit
    this->Tworld = this->LogStates.back().Isometry;
    this->TworldInit = this->LogStates.front().Isometry;

    // Update maps from the beginning using the new trajectory
    this->UpdateMaps(true);
  }

  PRINT_VERBOSE(1, "Pose initialized with external pose measurement");
  return true;
}

//-----------------------------------------------------------------------------
void Slam::ComputeEgoMotion()
{
  PRINT_VERBOSE(2, "========== Ego-Motion ==========");

  if (this->LogStates.empty())
  {
    PRINT_WARNING("The LogStates is empty : cannot use them to compute ego motion")
    return;
  }

  // Reset ego-motion
  this->Trelative = Eigen::Isometry3d::Identity();

  bool externalAvailable = false;
  // Use external poses to estimate new pose
  if (!this->LogStates.empty() &&
      (this->EgoMotion == EgoMotionMode::EXTERNAL ||
       this->EgoMotion == EgoMotionMode::EXTERNAL_OR_MOTION_EXTRAPOLATION))
  {
    if (this->PoseHasData())
    {
      ExternalSensors::PoseMeasurement synchPreviousPoseMeas; // Virtual measure with synchronized timestamp and calibration applied
      if (this->PoseManager->ComputeSynchronizedMeasureBase(this->LogStates.back().Time, synchPreviousPoseMeas))
      {
        ExternalSensors::PoseMeasurement synchPoseMeas; // Virtual measure with synchronized timestamp and calibration applied
        if (this->PoseManager->ComputeSynchronizedMeasureBase(this->CurrentTime, synchPoseMeas))
        {
          this->Trelative = synchPreviousPoseMeas.Pose.inverse() * synchPoseMeas.Pose;
          externalAvailable = true;
          PRINT_VERBOSE(3, "Prior pose computed using external poses supplied");
        }
      }
    }
    else
      PRINT_WARNING("External poses are empty : cannot use them to compute ego motion")
  }

  // Linearly extrapolate previous motion to estimate new pose
  if (this->LogStates.size() >= 2 &&
      (this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION ||
       this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION ||
       (this->EgoMotion == EgoMotionMode::EXTERNAL_OR_MOTION_EXTRAPOLATION && !externalAvailable)))
  {
    // Estimate new Tworld with a constant velocity model
    // Get last pose (called #1)
    const double t1 = this->LogStates.back().Time;
    const Eigen::Isometry3d& T1 = this->LogStates.back().Isometry;
    // Get second to last pose (called #0)
    LidarState& prevLastState = *std::prev(std::prev(this->LogStates.end()));
    // Check there wasn't any failure nor recovery in the last states
    if (!this->RecoveryTimes.empty() &&
        (prevLastState.Time <= this->RecoveryTimes.back() ||
         this->LogStates.back().Time <= this->RecoveryTimes.back()))
      PRINT_WARNING("SLAM has not fully recovered : unable to extrapolate scan pose from previous motion.")
    else
    {
      const double t0 = prevLastState.Time;
      const Eigen::Isometry3d& T0 = prevLastState.Isometry;
      // Check times
      if (std::abs(Utils::Normalize(this->CurrentTime, t0, t1)) > this->MaxExtrapolationRatio)
        PRINT_WARNING("Unable to extrapolate scan pose from previous motion : extrapolation time is too far.")
      else
      {
        // Estimate new Tworld with a linear extrapolation
        Eigen::Isometry3d nextTworldEstimation = Interpolation::LinearInterpo({T0,t0}, {T1, t1}, this->CurrentTime);
        this->Trelative = this->Tworld.inverse() * nextTworldEstimation;
      }
    }
  }

  // Refine Trelative estimation by registering current frame on previous one
  if (this->EgoMotion == EgoMotionMode::REGISTRATION ||
      this->EgoMotion == EgoMotionMode::MOTION_EXTRAPOLATION_AND_REGISTRATION)
  {
    // kd-tree to process fast nearest neighbor
    // among the keypoints of the previous pointcloud
    IF_VERBOSE(3, Utils::Timer::Init("EgoMotion : build KD tree"));

    // Build a new submap for PreviousRawKeypoints
    Maps previousKeypoints;
    this->InitSubMaps(previousKeypoints);
    // Reduce the leaf size to get all the keypoints from the previous frame
    for (auto k : this->UsableKeypoints)
      previousKeypoints[k]->SetLeafSize(0.05);
    for (auto k : this->UsableKeypoints)
      previousKeypoints[k]->Add(this->PreviousRawKeypoints[k]);

    // The iteration is not directly on Keypoint types
    // because of openMP behaviour which needs int iteration on MSVC
    int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
    // Build a kd-tree for previousKeypoints maps
    #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
    for (int i = 0; i < nbKeypointTypes; ++i)
    {
      Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
      if (!previousKeypoints[k]->IsSubMapKdTreeValid())
        previousKeypoints[k]->BuildSubMapKdTree();
    }

    if (this->Verbosity >= 2)
    {
      std::cout << "Keypoints extracted from previous frame : ";
      for (auto k : this->UsableKeypoints)
        std::cout << previousKeypoints[k]->GetSubMapKdTree().GetInputCloud()->size() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
      std::cout << std::endl;
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("EgoMotion : build KD tree"));
    IF_VERBOSE(3, Utils::Timer::Init("Ego-Motion : whole ICP-LM loop"));

    // Set localization parameters which do not have a setter
    this->EgoMotionParams.MatchingParams.NbThreads = static_cast<unsigned int>(this->NbThreads);
    this->EgoMotionParams.MatchingParams.SingleEdgePerRing = true;
    // ICP - Levenberg-Marquardt loop to update Trelative
    this->EstimatePose(this->CurrentRawKeypoints, previousKeypoints,
                       this->EgoMotionParams, this->Trelative,
                       this->EgoMotionMatchingResults);

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Ego-Motion : whole ICP-LM loop"));
    if (this->Verbosity >= 2)
    {
      std::cout << "Matched keypoints: " << this->TotalMatchedKeypoints << " (";
      for (auto k : this->UsableKeypoints)
        std::cout << this->EgoMotionMatchingResults[k].NbMatches() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
      std::cout << ")" << std::endl;
    }
  }

  // Print EgoMotion results
  SET_COUT_FIXED_PRECISION(3);
  PRINT_VERBOSE(2, "Estimated Ego-Motion (motion since last frame):\n"
                   " translation = [" << this->Trelative.translation().transpose() << "] m\n"
                   " rotation    = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(this->Trelative.linear())).transpose() << "] °");
  RESET_COUT_FIXED_PRECISION;
}

//-----------------------------------------------------------------------------
void Slam::Localization()
{
  this->LocalizationUncertainty = LocalOptimizer::RegistrationError();
  this->OptimizationValid = true;
  PRINT_VERBOSE(2, "========== Localization ==========");

  // Integrate the relative motion to the world transformation
  // Store previous tworld for next iteration
  this->Tworld = this->Tworld * this->Trelative;

  if (!this->Undistortion)
    this->CurrentUndistortedKeypoints = this->CurrentRawKeypoints;
  else
  {
    IF_VERBOSE(3, Utils::Timer::Init("Localization : initial undistortion"));
    // Init undistorted keypoints clouds from raw points
    // Undistort the keypoints with current information to represent them in base frame at current time

    // The iteration is not directly on Keypoint types
    // because of openMP behaviour which needs int iteration on MSVC
    int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
    #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
    for (int i = 0; i < nbKeypointTypes; ++i)
    {
      Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
      // Sort points by time to speed up search
      if (!std::is_sorted(this->CurrentRawKeypoints[k]->points.begin(), this->CurrentRawKeypoints[k]->points.end(),
                         [](const LidarPoint& pt1, const LidarPoint& pt2){return pt1.time < pt2.time;}))
      {
        std::sort(this->CurrentRawKeypoints[k]->points.begin(), this->CurrentRawKeypoints[k]->points.end(),
                  [](const LidarPoint& pt1, const LidarPoint& pt2){return pt1.time < pt2.time;});
      }
      this->CurrentUndistortedKeypoints[k].reset(new PointCloud);
      *this->CurrentUndistortedKeypoints[k] = *this->CurrentRawKeypoints[k];

      if (this->Undistortion != UndistortionMode::EXTERNAL)
      {
        // Get base frame at current time in log states and represent all the points in this frame
        // using pose interpolation with selected model
        this->UndistortWithLogStates(this->CurrentRawKeypoints[k],
                                     this->CurrentUndistortedKeypoints[k],
                                     true); // true -> current Tworld is used to undistort with LogStates
      }
      else if (this->PoseHasData())
      {
        // Get base frame at current tme in pose measurements and represent all the points in this frame
        // using pose interpolation with selected model
        this->UndistortWithPoseMeasurement(this->CurrentUndistortedKeypoints[k],
                                           this->CurrentTime);
      }
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : initial undistortion"));
  }

  // Get keypoints from maps and build kd-trees for fast nearest neighbors search
  IF_VERBOSE(3, Utils::Timer::Init("Localization : map keypoints extraction"));

  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
    // Check the current frame contains not null number of k type keypoints
    if (this->CurrentUndistortedKeypoints[k] && this->CurrentUndistortedKeypoints[k]->empty())
      continue;
    // If the map has been updated, the KD-tree needs to be updated
    if (!this->LocalMaps[k]->IsSubMapKdTreeValid())
    {
      // If maps are fixed, we can build a single KD-tree
      // of the entire map to avoid rebuilding it again
      if (this->MapUpdate == MappingMode::NONE)
        this->LocalMaps[k]->BuildSubMapKdTree();

      // Otherwise, we only extract the local sub maps
      // to build a local and smaller KD-tree
      else
      {
        if (this->LocalMaps[k]->IsTimeThreshold())
        {
          IF_VERBOSE(3, Utils::Timer::Init("Localization : clearing old points"));
          this->LocalMaps[k]->ClearPoints(this->CurrentTime);
          IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : clearing old points"));
        }
        // Estimate current keypoints bounding box
        PointCloud currWorldKeypoints;
        pcl::transformPointCloud(*this->CurrentUndistortedKeypoints[k], currWorldKeypoints, this->Tworld.matrix());
        Eigen::Vector4f minPoint, maxPoint;
        pcl::getMinMax3D(currWorldKeypoints, minPoint, maxPoint);

        // Build submap of all points lying in this bounding box
        // Moving objects are rejected but the constraint is removed
        // if less than half the number of current keypoints are extracted from the map
        this->LocalMaps[k]->BuildSubMapKdTree(minPoint.head<3>().array(), maxPoint.head<3>().array(), currWorldKeypoints.size() / 2);
      }
    }
  }

  if (this->Verbosity >= 2)
  {
    std::cout << "Keypoints extracted from map : ";
    for (auto k : this->UsableKeypoints)
    {
      std::cout << this->LocalMaps[k]->GetSubMapKdTree().GetInputCloud()->size()
                << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    }
    std::cout << std::endl;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : map keypoints extraction"));
  IF_VERBOSE(3, Utils::Timer::Init("Localization : whole ICP-LM loop"));

  // Set localization parameters which do not have a setter
  this->LocalizationParams.MatchingParams.NbThreads = static_cast<unsigned int>(this->NbThreads);
  this->LocalizationParams.MatchingParams.SingleEdgePerRing = false;
  // ICP - Levenberg-Marquardt loop to update Tworld
  this->LocalizationUncertainty = this->EstimatePose(this->CurrentUndistortedKeypoints,
                                                     this->LocalMaps,
                                                     this->LocalizationParams,
                                                     this->Tworld,
                                                     this->LocalizationMatchingResults);
  this->OptimizationValid = this->LocalizationUncertainty.Valid;

  // Reset state to previous one to avoid instability
  if (!this->OptimizationValid)
    this->Tworld = this->LogStates.empty()? this->TworldInit : Eigen::Isometry3d(this->LogStates.back().Isometry);

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Localization : whole ICP-LM loop"));

  // Optionally print localization optimization summary
  if (this->Verbosity >= 2)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "Matched keypoints: " << this->TotalMatchedKeypoints << " (";
    for (auto k : this->UsableKeypoints)
      std::cout << this->LocalizationMatchingResults[k].NbMatches() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";

    std::cout << ")"
              << "\nPosition uncertainty    = " << this->LocalizationUncertainty.PositionError    << " m"
              << " (along [" << this->LocalizationUncertainty.PositionErrorDirection.transpose()    << "])"
              << "\nOrientation uncertainty = " << this->LocalizationUncertainty.OrientationError << " °"
              << " (along [" << this->LocalizationUncertainty.OrientationErrorDirection.transpose() << "])"
              << std::endl;
    RESET_COUT_FIXED_PRECISION;
  }
}

//-----------------------------------------------------------------------------
bool Slam::NeedNewKeyFrame()
{
  // Compute motion since last keyframe
  Eigen::Isometry3d motionSinceLastKf = this->KfLastPose.inverse() * this->Tworld;
  double transSinceLastKf = motionSinceLastKf.translation().norm();
  double rotSinceLastKf = Eigen::AngleAxisd(motionSinceLastKf.linear()).angle();
  PRINT_VERBOSE(3, "Motion since last keyframe #" << this->KfCounter << ": "
                                                  << transSinceLastKf << " m, "
                                                  << Utils::Rad2Deg(rotSinceLastKf) << " °");
  // Check if current frame is a new keyframe
  // If we don't have enough keyframes yet, the threshold is linearly lowered
  constexpr double MIN_KF_NB = 10.;
  double thresholdCoef = std::min(this->KfCounter / MIN_KF_NB, 1.);

  // Mark as keyframe if a new tag was seen after some time
  // This allows to force some keyframes and therefore a constraint in the pose graph optimization
  // if the tag detections are quite sparse
  bool tagRequirement = false;
  for (auto& idLm : this->LandmarksManagers)
  {
    if (idLm.second.NeedsReferencePoseRefresh(this->CurrentTime))
    {
      tagRequirement = true;
      break;
    }
  }

  return transSinceLastKf >= thresholdCoef * this->KfDistanceThreshold ||
         rotSinceLastKf >= Utils::Deg2Rad(thresholdCoef * this->KfAngleThreshold) ||
         tagRequirement;
}

//-----------------------------------------------------------------------------
void Slam::UpdateMapsUsingTworld()
{
  PRINT_VERBOSE(3, "Adding new keyframe #" << this->KfCounter);

  // Transform keypoints to WORLD coordinates
  for (auto k : this->UsableKeypoints)
    this->CurrentWorldKeypoints[k] = this->GetKeypoints(k, true);

  // Add registered points to map
  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
    // Add not fixed points
    this->LocalMaps[k]->Add(this->CurrentWorldKeypoints[k], false);
  }
}

//-----------------------------------------------------------------------------
void Slam::LogCurrentFrameState()
{
  // Number of data to keep in order to perform interpolation
  // The std::max avoid underflow
  size_t nbDataToKeep = std::max(Interpolation::ModelRequiredNbData.at(this->Interpolation), 1u) - 1u;
  // Last poses are logged in any case for motion extrapolation and undistortion
  // If LogOnlyKeyframes is required, remove the no keyframe logStates (those before required logStates)
  if (this->LogStates.size() > nbDataToKeep && this->LogOnlyKeyframes)
  {
    auto itLastRequired = std::prev(this->LogStates.end(), nbDataToKeep);
    auto itSt = this->LogStates.begin();
    while (itSt != itLastRequired)
      itSt = itSt->IsKeyFrame ? ++itSt : this->LogStates.erase(itSt);
  }
  // Save current state to log buffer.
  // This buffer will be processed in the pose graph optimization
  // and is used locally to compute prior ego-motion estimation
  // and to undistort the input points
  LidarState state;
  state.Isometry = this->Tworld;
  // Increase covariance area by scale
  state.Covariance = std::pow(this->CovarianceScale, 2) * this->LocalizationUncertainty.Covariance;
  float defaultPositionError = 1e-2; // 1cm
  float defaultAngleError = Utils::Deg2Rad(1.f); // 1°
  if (!Utils::isCovarianceValid(state.Covariance))
    state.Covariance = Utils::CreateDefaultCovariance(defaultPositionError, defaultAngleError); //1mm, 0.5°

  // If 2D mode enabled, supply constant covariance for unevaluated variables
  if (this->TwoDMode)
  {
    state.Covariance(2, 2) = std::pow(defaultPositionError, 2);
    state.Covariance(3, 3) = std::pow(defaultAngleError,    2);
    state.Covariance(4, 4) = std::pow(defaultAngleError,    2);
  }
  state.Time = this->CurrentTime;
  state.Index = this->NbrFrameProcessed;
  state.IsKeyFrame = this->IsKeyFrame;
  for (auto k : this->UsableKeypoints)
  {
    state.Keypoints[k] = std::make_shared<PCStorage>(this->CurrentUndistortedKeypoints[k], this->LoggingStorage);
    state.RawKeypoints[k] = std::make_shared<PCStorage>(this->CurrentRawKeypoints[k], this->LoggingStorage);
  }

  this->LogStates.emplace_back(state);
  // Remove the oldest logged states
  auto itSt = this->LogStates.begin();
  while (this->CurrentTime - itSt->Time > this->LoggingTimeout &&
         this->LogStates.size() > nbDataToKeep + 1)
  {
    ++itSt;
    this->LogStates.pop_front();
  }
}

//-----------------------------------------------------------------------------
void Slam::SetUndistortion(UndistortionMode undistMode)
{
  this->Undistortion = undistMode;
  this->LocalizationParams.Undistortion = undistMode;
}

//-----------------------------------------------------------------------------
std::vector<LidarState> Slam::GetLastStates(double freq)
{
  if (this->LogStates.empty())
  {
    LidarSlam::LidarState state;
    state.Isometry = this->TworldInit;
    return {state.Isometry};
  }

  if (freq < 0 || this->LogStates.size() == 1)
    return {this->LogStates.back()};

  LidarState last     = *(std::prev(this->LogStates.end()));
  LidarState prevLast = *(std::prev(std::prev(this->LogStates.end())));

  if (std::abs(last.Time - prevLast.Time) > 1.0)
  {
    PRINT_WARNING("Unable to interpolate between states : time is too far");
    return {last};
  }

  if (last.Time < prevLast.Time)
    std::swap(last, prevLast);

  double period = 1. / freq;

  double time = prevLast.Time + period;
  std::vector<LidarState> lastStates;
  lastStates.reserve(std::ceil((last.Time + 1e-6 - prevLast.Time) * freq));

  // Create interpolation model from last states
  unsigned int nbRequiredData = Interpolation::ModelRequiredNbData.at(this->Interpolation);
  std::vector<PoseStamped> ctrlPoses = this->GetStatesWindow(std::prev(this->LogStates.end()), nbRequiredData);
  Interpolation::Trajectory interpolator(this->Interpolation, ctrlPoses);

  while (time < last.Time - std::min(1e-6, period))
  {
    LidarSlam::LidarState state;
    state.Time = time;
    state.Covariance = last.Covariance;
    if (this->PoseHasData())
      state.Isometry = this->GetTworld(time, true);
    else
      state.Isometry = interpolator(time);
    lastStates.emplace_back(state);
    time += period;
  }
  lastStates.emplace_back(last);

  return lastStates;
}

//==============================================================================
//   Loop Closure usage
//==============================================================================

//-----------------------------------------------------------------------------
bool Slam::DetectLoopClosureIndices(std::list<LidarState>::iterator& itQueryState, std::list<LidarState>::iterator& itRevisitedState)
{
  PRINT_VERBOSE(2, "========== Loop closure : Detection ==========");
  bool detectionValid = false;
  switch (this->LoopParams.Detector)
  {
    case LoopClosureDetector::NONE:
    {
      PRINT_WARNING("Loop closure detection is disabled!");
      return false;
    }
    case LoopClosureDetector::MANUAL:
    {
      // If the detector is manual, check whether or not the input frames are stored in the LogStates
      // When LogOnlyKeyFrames is enabled, only keyframes are stored in the LogStates.
      // It is possible that the inputs frame indices are not keyframes.
      // In this case, replace the input frame index by its neighbor keyframe
      itQueryState = itRevisitedState = this->LogStates.begin();
      while (itQueryState->Index < this->LoopParams.QueryIdx && itQueryState->Index != this->LogStates.back().Index)
        ++itQueryState;
      if (itQueryState->Index != this->LoopParams.QueryIdx)
      {
        this->LoopParams.QueryIdx = itQueryState->Index;
        PRINT_WARNING("The input query frame index is not found in Logstates and is replaced by frame #"
                      << this->LoopParams.QueryIdx << ".");
      }

      while (itRevisitedState->Index < this->LoopParams.RevisitedIdx && itRevisitedState->Index != this->LogStates.back().Index)
        ++itRevisitedState;
      if (itRevisitedState->Index != this->LoopParams.RevisitedIdx)
      {
        this->LoopParams.RevisitedIdx = itRevisitedState->Index;
        PRINT_WARNING("The input revisited frame index is not found in Logstates and is replaced by frame #"
                      << this->LoopParams.RevisitedIdx << ".");
      }
      PRINT_VERBOSE(3, "Loop closure is detected by external information. The relevant frame indices are:\n"
                    << " Query frame #" << this->LoopParams.QueryIdx << " Revisited frame #" << this->LoopParams.RevisitedIdx);
      detectionValid = true;
      break;
    }
    case LoopClosureDetector::TEASERPP:
    {
      // Automatic detection of loop closure by teaserpp registration
      // It detects automatically a revisited frame idx for the current frame
      itQueryState = std::prev(this->LogStates.end());
      itRevisitedState = this->LogStates.begin();
      detectionValid = this->DetectLoopWithTeaser(itQueryState, itRevisitedState);
      if (detectionValid)
      {
        this->LoopParams.QueryIdx = itQueryState->Index;
        this->LoopParams.RevisitedIdx = itRevisitedState->Index;
      }
      break;
    }
  }

  return detectionValid;
}

//-----------------------------------------------------------------------------
bool Slam::DetectLoopWithTeaser(std::list<LidarState>::iterator& itQueryState, std::list<LidarState>::iterator& itRevisitedState)
{
  #ifdef USE_TEASERPP
  // Create query submap and get keypoints in BASE coordinates
  Maps querySubMaps;
  this->InitSubMaps(querySubMaps);
  this->BuildMaps(querySubMaps,
                  this->FetchStateIndex(itQueryState, this->LoopParams.QueryMapStartRange)->Index,
                  this->FetchStateIndex(itQueryState, this->LoopParams.QueryMapEndRange)->Index,
                  itQueryState->Index);

  // Aggregate keypoints
  PointCloud::Ptr queryPoints(new PointCloud);
  for (auto k : this->UsableKeypoints)
    *queryPoints += *querySubMaps[k]->Get();
  // Compute FPFH features for query keypoints
  auto queryDescriptors = this->ComputeFPFHFeatures(queryPoints,
                                                    2 * this->GetVoxelGridLeafSize(PLANE),
                                                    3 * this->GetVoxelGridLeafSize(PLANE));

  // Convert query submap keypoints into teaser registration input format
  teaser::PointCloud teaserQueryPoints;
  teaserQueryPoints.reserve(queryPoints->size());
  #pragma omp parallel for num_threads(this->NbThreads)
  for (const auto& pt: *queryPoints)
    teaserQueryPoints.push_back({pt.x, pt.y, pt.z}); // Note: only push_back can be used for teaser pointcloud format

  // Compute the frame index relative to the newest frame onto which a loop closure is searched
  // A frame gap is set to avoid finding a loop closure between the query frame and its direct surrounding.
  int lastFrameIdx = this->FetchStateIndex(itQueryState, -this->LoopParams.GapLength)->Index;

  // Search the revisited frame by teaser registration
  auto itSt = this->LogStates.begin();
  // Temporary value to evaluate the overlap for each candidate
  float overlap = 0.;
  // Temporary value to store the transform of the best teaser registration
  Eigen::Isometry3d queryPose = Eigen::Isometry3d::Identity();
  while (itSt->Index < static_cast<unsigned int>(lastFrameIdx) && itSt != this->LogStates.end())
  {
    // Create candidate submap
    Maps candidateSubMaps;
    this->InitSubMaps(candidateSubMaps);
    if (this->LoopParams.SampleStep < 0)
      // Use the whole map as target
      this->BuildMaps(candidateSubMaps, itSt->Index, lastFrameIdx);
    else
      // Use the submap between RevisitedMapStartRange and RevisitedMapEndRange as target
      this->BuildMaps(candidateSubMaps,
                      this->FetchStateIndex(itSt, this->LoopParams.RevisitedMapStartRange)->Index,
                      this->FetchStateIndex(itSt, this->LoopParams.RevisitedMapEndRange)->Index);

    // Compute FPFH features for candidate keypoints
    PointCloud::Ptr candidatePoints(new PointCloud);
    for (auto k : this->UsableKeypoints)
      *candidatePoints += *candidateSubMaps[k]->Get();
    auto candidateDescriptors = this->ComputeFPFHFeatures(candidatePoints,
                                                          2 * this->GetVoxelGridLeafSize(PLANE),
                                                          3 * this->GetVoxelGridLeafSize(PLANE));

    // Calculate correspondences between query submap and candidate submap
    auto correspondences = this->CalculateCorrespondences(queryDescriptors, candidateDescriptors);

    // TEASER++ registration
    // Convert candidate submap keypoints into teaser registration input format
    teaser::PointCloud teaserCandidatePoints;
    teaserCandidatePoints.reserve(candidatePoints->size());
    #pragma omp parallel for num_threads(this->NbThreads)
    for (const auto& pt: *candidatePoints)
      teaserCandidatePoints.push_back({pt.x, pt.y, pt.z}); // Note: push_back is defined in teaserpp lib for its pointcloud

    // Solve with TEASER++
    teaser::RobustRegistrationSolver solver(this->LoopParams.TeaserParams);
    solver.solve(teaserQueryPoints, teaserCandidatePoints, correspondences);
    auto solution = solver.getSolution();
    Eigen::Isometry3d candidateLCTransform = Eigen::Isometry3d::Identity();
    candidateLCTransform.translation() = solution.translation;
    candidateLCTransform.linear() = solution.rotation;

    // Registration evaluation
    // Register query points in world using estimated LC transform
    PointCloud::Ptr worldQueryPoints(new PointCloud);
    pcl::transformPointCloud(*queryPoints, *worldQueryPoints, candidateLCTransform.matrix().cast<float>());

    // Build kdtree for candidate sub maps
    for (auto k : this->UsableKeypoints)
      candidateSubMaps[k]->BuildSubMapKdTree();

    // Compute LCP like estimator
    // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info)
    float overlapEstimation = Confidence::LCPEstimator(worldQueryPoints, candidateSubMaps,
                                                       this->GetVoxelGridLeafSize(PLANE), this->NbThreads, false);
    if (overlapEstimation > overlap)
    {
      overlap = overlapEstimation;
      itRevisitedState = itSt;
      queryPose = candidateLCTransform;
    }

    // Update iterator
    // If sample step is negative, no loop is actually needed
    if (this->LoopParams.SampleStep <= 1e-6)
      break;
    itSt = this->FetchStateIndex(itSt, this->LoopParams.SampleStep);
  }

  if (overlap >= this->LoopParams.EvaluationThreshold)
  {
    this->LoopDetectionTransform = queryPose;
    PRINT_VERBOSE(1, "Loop closure is detected for frame #"
                  << itQueryState->Index << " by teaserpp with "
                  << 100 * overlap << "% overlap.\n"
                  << "The revisited frame is #" << itRevisitedState->Index);
    return true;
  }
  else
  {
    this->LoopDetectionTransform = Eigen::Isometry3d::Identity();
    PRINT_ERROR("Loop closure is NOT detected for frame #" << itQueryState->Index << " by teaserpp.\n"
                 << "The detected frame (#" << itRevisitedState->Index << ") has a low overlap ("<< 100 * overlap << "%) ");
    return false;
  }
  #else
  static_cast<void>(itQueryState);
  static_cast<void>(itRevisitedState);

  PRINT_ERROR("Automatic loop closure detection requires TEASER++, but it was not found.");
  return false;
  #endif
}

#ifdef USE_TEASERPP
//-----------------------------------------------------------------------------
pcl::PointCloud<pcl::FPFHSignature33>::Ptr Slam::ComputeFPFHFeatures(const PointCloud::Ptr inputCloud,
                                                                     double normalSearchRadius,
                                                                     double fpfhSearchRadius)
{
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr descriptors(new pcl::PointCloud<pcl::FPFHSignature33>());

  // Estimate normals
  pcl::NormalEstimationOMP<Slam::Point, pcl::Normal> normalEstimation;
  normalEstimation.setInputCloud(inputCloud);
  normalEstimation.setRadiusSearch(normalSearchRadius);
  pcl::search::KdTree<Slam::Point>::Ptr kdtree(new pcl::search::KdTree<Slam::Point>);
  normalEstimation.setSearchMethod(kdtree);
  normalEstimation.compute(*normals);

  // Estimate FPFH
  pcl::FPFHEstimationOMP<Slam::Point, pcl::Normal, pcl::FPFHSignature33> fpfh;
  fpfh.setInputCloud(inputCloud);
  fpfh.setInputNormals(normals);
  fpfh.setSearchMethod(kdtree);
  fpfh.setRadiusSearch(fpfhSearchRadius);
  fpfh.compute(*descriptors);

  return descriptors;
}

//-----------------------------------------------------------------------------
std::vector<std::pair<int, int>> Slam::CalculateCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr sourceFeatures,
                                                                pcl::PointCloud<pcl::FPFHSignature33>::Ptr targetFeatures)
{
  std::vector<std::pair<int, int>> corres;

  // Compare the size of two input data and mark the size
  int nSource = static_cast<int>(sourceFeatures->size());
  int nTarget = static_cast<int>(targetFeatures->size());
  bool swapped = nSource < nTarget ? true : false;
  int nSmall = swapped ? nSource : nTarget;
  int nLarge = swapped ? nTarget : nSource;
  auto &featuresSmall = swapped ? sourceFeatures : targetFeatures;
  auto &featuresLarge = swapped ? targetFeatures : sourceFeatures;

  // Build kdtree for two input cloud
  pcl::search::KdTree<pcl::FPFHSignature33>::Ptr kdtreeSmall(new pcl::search::KdTree<pcl::FPFHSignature33>);
  pcl::search::KdTree<pcl::FPFHSignature33>::Ptr kdtreeLarge(new pcl::search::KdTree<pcl::FPFHSignature33>);
  kdtreeSmall->setInputCloud(featuresSmall);
  kdtreeLarge->setInputCloud(featuresLarge);

  // Initial matching
  // Parameters for nearest K search
  int kNeighbor = 1;
  std::vector<float> dis;
  std::vector<int> corresK;
  // large2Small is a vector to save the correspondence index in the small data set for each feature in the large dataset
  // small2Large is similiar to large2Small
  std::vector<int> large2Small(nLarge, -1);
  std::vector<int> small2Large(nSmall, -1);
  for (int idxS = 0; idxS < nSmall; idxS++)
  {
    // For each feature in the small dataset, search its nearest neighbor in the large dataset
    kdtreeLarge->nearestKSearch (featuresSmall->points[idxS], kNeighbor, corresK, dis);
    int idx = corresK[0];
    small2Large[idxS] = idx;
    if (large2Small[idx] == -1)
    {
      // For a feature index found in large dataset; search its nearest neighbor in the small dataset
      kdtreeSmall->nearestKSearch (featuresLarge->points[idx], kNeighbor, corresK, dis);
      large2Small[idx] = corresK[0];
    }
  }

  // Cross check the match is bijective
  for (int idxL = 0; idxL < nLarge; idxL++)
  {
    int idxS = large2Small[idxL];
    if (idxS == -1 )
      continue;
    if (small2Large[idxS] == idxL)
      corres.push_back(std::pair<int, int>(idxL, idxS));
  }

  if (swapped)
  {
    for (auto& c : corres)
        std::swap(c.first, c.second);
  }

  return corres;
}
#endif

//-----------------------------------------------------------------------------
bool Slam::LoopClosureRegistration(std::list<LidarState>::iterator& itQueryState,
                                   std::list<LidarState>::iterator& itRevisitedState,
                                   Eigen::Isometry3d& loopClosureTransform,
                                   Eigen::Matrix6d& loopClosureCovariance)
{
  // Optimization results of loop closure localization
  LocalOptimizer::RegistrationError loopClosureUncertainty = LocalOptimizer::RegistrationError();
  PRINT_VERBOSE(2, "========== Loop closure : Registration ==========");

  // Create a submap around revisited frame where a loop is detected
  Maps loopClosureRevisitedSubMaps;
  this->InitSubMaps(loopClosureRevisitedSubMaps);
  this->BuildMaps(loopClosureRevisitedSubMaps,
                  this->FetchStateIndex(itRevisitedState, this->LoopParams.RevisitedMapStartRange)->Index,
                  this->FetchStateIndex(itRevisitedState, this->LoopParams.RevisitedMapEndRange)->Index);
  PRINT_VERBOSE(3, "Sub maps are created around revisited frame #" << itRevisitedState->Index << ".");

  // Pose prior for optimization
  Eigen::Isometry3d loopClosureTworld = itQueryState->Isometry;
  if (!this->LoopDetectionTransform.matrix().isIdentity())
    loopClosureTworld = this->LoopDetectionTransform;
  else if (this->LoopParams.EnableOffset)
  {
    // Enable to add an offset to the pose prior when two poses are too far from each other.
    PointCloud::Ptr revisitedPlaneKeypoints(new PointCloud);
    pcl::transformPointCloud(*(itRevisitedState->Keypoints[PLANE]->GetCloud()),
                             *revisitedPlaneKeypoints,
                             itRevisitedState->Isometry.matrix().cast<float>());
    Eigen::Vector4f minPoint, maxPoint, midPoint;
    pcl::getMinMax3D(*revisitedPlaneKeypoints, minPoint, maxPoint);
    midPoint = 0.5 * (minPoint + maxPoint);
    loopClosureTworld.translation() = midPoint.head<3>().cast<double>();
    PRINT_VERBOSE(3, "An offset is added onto the query submaps.");
  }

  // If LoopClosureICPWithSubmap is enabled, create a sub map of keypoints around the query frame
  // Otherwise, use only keypoints of query frame as query keypoints
  // loopClosureQueryKeypoints are in BASE coordinates of query frame
  std::map<Keypoint, PointCloud::Ptr> loopClosureQueryKeypoints;
  for (auto k : this->UsableKeypoints)
    loopClosureQueryKeypoints[k].reset(new PointCloud);
  if (this->LoopParams.ICPWithSubmap)
  {
    Maps loopClosureQuerySubMaps;
    this->InitSubMaps(loopClosureQuerySubMaps);
    this->BuildMaps(loopClosureQuerySubMaps,
                    this->FetchStateIndex(itQueryState, this->LoopParams.QueryMapStartRange)->Index,
                    this->FetchStateIndex(itQueryState, this->LoopParams.QueryMapEndRange)->Index,
                    itQueryState->Index);
    PRINT_VERBOSE(3, "Sub maps are created around query frame #" << itQueryState->Index << ".");
    for (auto k : this->UsableKeypoints)
      loopClosureQueryKeypoints[k] = loopClosureQuerySubMaps[k]->Get();
  }
  else
  {
    for (auto k : this->UsableKeypoints)
      loopClosureQueryKeypoints[k] = itQueryState->Keypoints[k]->GetCloud();
  }

  IF_VERBOSE(3, Utils::Timer::Init("Loop closure Registration : submap keypoints extraction"));
  // Build kd-trees for fast nearest neighbors search from keypoints of loop closure sub map
  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
    if (!loopClosureRevisitedSubMaps[k]->IsSubMapKdTreeValid())
      loopClosureRevisitedSubMaps[k]->BuildSubMapKdTree();
  }

  if (this->Verbosity >= 2)
  {
    std::cout << "Keypoints extracted from loop closure sub map : ";
    for (auto k : this->UsableKeypoints)
      std::cout << loopClosureRevisitedSubMaps[k]->GetSubMapKdTree().GetInputCloud()->size()
                << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";
    std::cout << std::endl;
  }

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Loop closure Registration : submap keypoints extraction"));
  IF_VERBOSE(3, Utils::Timer::Init("Loop closure Registration : whole ICP-LM loop"));

  // Reset ICP results
  this->TotalMatchedKeypoints = 0;

  // Set loop closure parameters which do not have a setter
  this->LoopParams.OptParams.MatchingParams.NbThreads = static_cast<unsigned int>(this->NbThreads);
  this->LoopParams.OptParams.MatchingParams.SingleEdgePerRing = false;
  // ICP - Levenberg-Marquardt loop to estimate the pose of the current frame relatively to the close loop frame
  std::map<Keypoint, KeypointsMatcher::MatchingResults> loopMatchingResults;
  loopClosureUncertainty = this->EstimatePose(loopClosureQueryKeypoints, loopClosureRevisitedSubMaps,
                                              this->LoopParams.OptParams, loopClosureTworld,
                                              loopMatchingResults);

  IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Loop closure Registration : whole ICP-LM loop"));

  if (!loopClosureUncertainty.Valid)
  {
    PRINT_ERROR("Loop closure registration failed.")
    return loopClosureUncertainty.Valid;
  }

  // Get covariance
  Eigen::Matrix6d covariance = std::pow(this->CovarianceScale, 2) * loopClosureUncertainty.Covariance;
  float defaultPositionError = 1e-2; // 1cm
  float defaultAngleError = Utils::Deg2Rad(1.f); // 1°
  if (!Utils::isCovarianceValid(covariance))
    covariance = Utils::CreateDefaultCovariance(defaultPositionError, defaultAngleError);
  // If 2D mode enabled, supply constant covariance for unevaluated variables
  if (this->TwoDMode)
  {
    covariance(2, 2) = std::pow(defaultPositionError, 2);
    covariance(3, 3) = std::pow(defaultAngleError,    2);
    covariance(4, 4) = std::pow(defaultAngleError,    2);
  }
  // Compute the relative transform between revisited frame and query frame
  Eigen::Isometry3d revisitedStateInv = itRevisitedState->Isometry.inverse();
  loopClosureTransform = revisitedStateInv * loopClosureTworld;
  // Rotate covariance
  // Lidar Slam gives the covariance expressed in the map frame
  // We want the covariance expressed in the query frame to be consistent with supplied relative transform
  Eigen::Vector6d xyzrpy = Utils::IsometryToXYZRPY(loopClosureTworld);
  loopClosureCovariance = CeresTools::RotateCovariance(xyzrpy, covariance, revisitedStateInv, true); // new = revisitedState^-1 * loopClosureTworld

  // Optionally print loop closure registration optimization summary
  if (this->Verbosity >= 2)
  {
    SET_COUT_FIXED_PRECISION(3);
    std::cout << "Loop closure matched keypoints: " << this->TotalMatchedKeypoints << " (";
    for (auto k : this->UsableKeypoints)
      std::cout << loopMatchingResults[k].NbMatches() << " " << Utils::Plural(KeypointTypeNames.at(k)) << " ";

    std::cout << ")"
              << "\nLoop closure position uncertainty    = " << loopClosureUncertainty.PositionError    << " m"
              << " (along [" << loopClosureUncertainty.PositionErrorDirection.transpose()    << "])"
              << "\nLoop closure orientation uncertainty = " << loopClosureUncertainty.OrientationError << " °"
              << " (along [" << loopClosureUncertainty.OrientationErrorDirection.transpose() << "])"
              << std::endl;
    std::cout << "Loop closure Transform:\n"
                 " position    = [" << loopClosureTworld.translation().transpose()                                        << "] m\n"
                 " orientation = [" << Utils::Rad2Deg(Utils::RotationMatrixToRPY(loopClosureTworld.linear())).transpose() << "] °" << std::endl;

    RESET_COUT_FIXED_PRECISION;
  }
  return loopClosureUncertainty.Valid;
}

//==============================================================================
//   Map helpers
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::InitSubMaps(Maps& maps)
{
  // Reset previous sub maps
  this->ClearMaps(maps);

  // Init SubMaps for each keypoint type with the same resolution as the one used in LocalMaps
  for (auto k : this->UsableKeypoints)
  {
    maps[k] = std::make_shared<RollingGrid>();
    maps[k]->SetVoxelResolution(this->LocalMaps[k]->GetVoxelResolution());
    maps[k]->SetGridSize(this->LocalMaps[k]->GetGridSize());
    maps[k]->SetLeafSize(this->LocalMaps[k]->GetLeafSize());
  }
}

//-----------------------------------------------------------------------------
void Slam::BuildMaps(Maps& maps, int windowStartIdx, int windowEndIdx, int idxFrame)
{
  // If default values of windowStartIdx and windowEndIdx are used, build maps with all frames stored in LogStates.
  // Otherwise, create a sub map with frames [windowStartIdx, windowEndIdx].
  unsigned int idxMin = static_cast<unsigned int>(std::max(0, windowStartIdx));
  unsigned int idxMax = windowEndIdx < 0 ? this->LogStates.back().Index : std::min(this->LogStates.back().Index, static_cast<unsigned int>(windowEndIdx));

  // Keypoints are aggregated in base coordinates of frame #idxFrame
  Eigen::Isometry3d currentBaseInv = Eigen::Isometry3d::Identity();
  if (idxFrame >= 0)
  {
    for (auto& state : this->LogStates)
    {
      if (state.Index == static_cast<unsigned int>(idxFrame))
      {
        currentBaseInv = state.Isometry.inverse();
        break;
      }
    }
  }

  // The iteration is not directly on Keypoint types
  // because of openMP behaviour which needs int iteration on MSVC
  int nbKeypointTypes = static_cast<int>(this->UsableKeypoints.size());
  #pragma omp parallel for num_threads(std::min(this->NbThreads, nbKeypointTypes))
  for (int i = 0; i < nbKeypointTypes; ++i)
  {
    Keypoint k = static_cast<Keypoint>(this->UsableKeypoints[i]);
    PointCloud::Ptr keypoints(new PointCloud);

    // Roll to center onto last pose to make sure slam can follow the trajectory after the maps have been updated
    Eigen::Vector4f minPoint, maxPoint;
    pcl::getMinMax3D(*this->LogStates.back().Keypoints[k]->GetCloud(), minPoint, maxPoint);
    maps[k]->Roll(minPoint.head<3>().array(), maxPoint.head<3>().array());

    for (auto& state : this->LogStates)
    {
      if (!state.IsKeyFrame || state.Index < idxMin)
        continue;
      if (state.Index > idxMax)
        break;
      // Keypoints are stored undistorted : because of the keyframes mechanism,
      // undistortion cannot be refined during pose graph
      // We rely on a good first estimation of the in-frame motion
      keypoints.reset(new PointCloud);
      PointCloud::Ptr undistortedKeypoints = state.Keypoints[k]->GetCloud();
      auto transform = idxFrame >= 0 ? currentBaseInv.matrix() * state.Isometry.matrix() : state.Isometry.matrix();
      pcl::transformPointCloud(*undistortedKeypoints, *keypoints, transform.cast<float>());
      maps[k]->Add(keypoints, false);
    }
  }
}

//-----------------------------------------------------------------------------
std::list<LidarState>::iterator Slam::FetchStateIndex(std::list<LidarState>::iterator startIt, double minDistance)
{
  if (minDistance == 0)
    return startIt;

  // itBound represents the first state that the trajectory length is not smaller than minDistance
  // between startIt and itBound
  auto itBound = startIt;
  auto itStop = minDistance < 0 ? this->LogStates.begin() : std::prev(this->LogStates.end());
  double trajLength = 0;
  while (trajLength < std::abs(minDistance) && itBound != itStop)
  {
    // Compute distance between two poses
    auto itNeighbor = minDistance < 0 ? std::prev(itBound) : std::next(itBound);
    trajLength += (itBound->Isometry.translation() - itNeighbor->Isometry.translation()).norm();
    itBound = itNeighbor;
  }
  return itBound;
}

//-----------------------------------------------------------------------------
LocalOptimizer::RegistrationError Slam::EstimatePose(const std::map<Keypoint, PointCloud::Ptr>& sourceKeypoints,
                                                     const Maps& targetKeypoints,
                                                     Optimization::Parameters& params,
                                                     Eigen::Isometry3d& posePrior,
                                                     std::map<Keypoint, KeypointsMatcher::MatchingResults>& matchingResults)
{
  LocalOptimizer::RegistrationError optimizationUncertainty = LocalOptimizer::RegistrationError();

  // Reset ICP results
  this->TotalMatchedKeypoints = 0;

  // ICP - Levenberg-Marquardt loop
  // At each step of this loop an ICP matching is performed. Once the keypoints
  // are matched, we estimate the 6-DOF parameters by minimizing the
  // non-linear least square cost function using Levenberg-Marquardt algorithm.
  for (unsigned int icpIter = 0; icpIter < params.ICPMaxIter; ++icpIter)
  {
    IF_VERBOSE(3, Utils::Timer::Init("  Pose estimation : ICP"));

    // We want to estimate our 6-DOF parameters using a non linear least square
    // minimization. The non linear part comes from the parametrization of the
    // rotation endomorphism SO(3).
    // First, we need to build the point-to-line, point-to-plane and
    // point-to-blob ICP matches that will be optimized.
    // Then, we use CERES Levenberg-Marquardt optimization to minimize problem.

    // Create a keypoints matcher
    // At each ICP iteration, the outliers removal is refined to be stricter
    double iterRatio = icpIter / static_cast<double>(params.ICPMaxIter - 1);
    params.MatchingParams.SaturationDistance = (1 - iterRatio) * params.InitSaturationDistance + iterRatio * params.FinalSaturationDistance;
    KeypointsMatcher matcher(params.MatchingParams, posePrior);

    // Loop over keypoints to build the point to line residuals
    this->TotalMatchedKeypoints = 0;
    for (auto k : this->UsableKeypoints)
    {
      matchingResults[k] = matcher.BuildMatchResiduals(sourceKeypoints.at(k), targetKeypoints.at(k)->GetSubMapKdTree(), k);
      this->TotalMatchedKeypoints += matchingResults[k].NbMatches();
    }

    // Skip frame if not enough keypoints are extracted
    if (this->TotalMatchedKeypoints < this->MinNbMatchedKeypoints)
    {
      PRINT_ERROR("Not enough keypoints matched, Pose estimation skipped for this frame.");
      optimizationUncertainty.Valid = false;
      break;
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Pose estimation : ICP"));
    IF_VERBOSE(3, Utils::Timer::Init("  Pose estimation : LM optim"));

    // Init the optimizer with initial pose and parameters
    LocalOptimizer optimizer;
    optimizer.SetTwoDMode(this->TwoDMode);
    optimizer.SetPosePrior(posePrior);
    optimizer.SetLMMaxIter(params.LMMaxIter);
    optimizer.SetNbThreads(this->NbThreads);

    // Add LiDAR ICP matches
    for (auto k : this->UsableKeypoints)
      optimizer.AddResiduals(matchingResults[k].Residuals);

    if (params.EnableExternalConstraints)
    {
      // Add odometry constraint
      // if constraint has been successfully created
      if (this->WheelOdomManager && this->WheelOdomManager->GetResidual().Cost)
        optimizer.AddResidual(this->WheelOdomManager->GetResidual());

      // Add gravity alignment constraint
      // if constraint has been successfully created
      if (this->GravityManager && this->GravityManager->GetResidual().Cost)
        optimizer.AddResidual(this->GravityManager->GetResidual());

      // Add Pose constraint
      // if constraint has been successfully created
      if (this->PoseManager && this->PoseManager->GetResidual().Cost)
        optimizer.AddResidual(this->PoseManager->GetResidual());

      // Add available landmark constraints
      for (auto& idLm : this->LandmarksManagers)
      {
        // Add landmark constraint
        // if constraint has been successfully created
        if (idLm.second.GetResidual().Cost)
          optimizer.AddResidual(idLm.second.GetResidual());
      }

      // Add camera constraints
      // if constraints have been successfully created
      if (this->CameraManager && this->CameraManager->GetResidual().Cost)
      {
        for (const auto& res : this->CameraManager->GetResiduals())
          optimizer.AddResidual(res);
      }
    }

    // Run LM optimization
    ceres::Solver::Summary summary = optimizer.Solve();
    PRINT_VERBOSE(4, summary.BriefReport());

    // Update pose prior from optimization results
    posePrior = optimizer.GetOptimizedPose();

    // Optionally refine undistortion
    if (params.Undistortion == UndistortionMode::REFINED)
    {
      for (auto k : this->UsableKeypoints)
        this->UndistortWithLogStates(this->CurrentRawKeypoints[k],
                                     this->CurrentUndistortedKeypoints[k],
                                     true); // true -> current Tworld is used to undistort with LogStates
    }

    IF_VERBOSE(3, Utils::Timer::StopAndDisplay("  Pose estimation : LM optim"));

    // If no L-M iteration has been made since the last ICP matching, it means
    // that we reached a local minimum for the ICP-LM algorithm.
    // We evaluate the quality of the Tworld optimization using an approximate
    // computation of the variance covariance matrix.
    if ((summary.num_successful_steps == 1) || (icpIter == params.ICPMaxIter - 1))
    {
      optimizationUncertainty = optimizer.EstimateRegistrationError();
      break;
    }
  }

  return optimizationUncertainty;
}

//==============================================================================
//   Undistortion helpers
//==============================================================================

std::list<LidarState>::const_iterator Slam::GetClosestState(double queryTime) const
{
  // Edge cases, should be well processed outside of this function
  if (this->LogStates.empty() || this->LogStates.size() == 1)
    return this->LogStates.begin();

  // Get first state after input time
  auto queryIt = std::upper_bound(this->LogStates.begin(),
                                  this->LogStates.end(),
                                  queryTime,
                                  [&](double t, const LidarState& state) {return t < state.Time;});

  // Deduce closest state to input time
  if (queryIt == this->LogStates.end())
    --queryIt;
  else if (queryIt != LogStates.begin())
  {
    if (std::abs(queryIt->Time - queryTime) > std::abs(std::prev(queryIt)->Time - queryTime))
      --queryIt;
  }
  return queryIt;
}

//-----------------------------------------------------------------------------
int Slam::GetTrajSection(double time) const
{
  if (this->RecoveryTimes.empty())
    return 0;

  int trajSection = 0;
  while (trajSection < this->RecoveryTimes.size() &&
         time < this->RecoveryTimes[trajSection])
    ++trajSection;

  return trajSection;
}

//-----------------------------------------------------------------------------
std::vector<PoseStamped> Slam::GetStatesWindow(std::list<LidarState>::const_iterator queryIt, unsigned int windowWidth) const
{
  // Check times validity depending on chronology
  // Times must follow the required chronology + not be too far
  // from each others.
  auto checkTimes = [&](double t1, double t2, int chrono = 0)
  {
    return ((chrono == 0 && std::abs(t2 - t1) > 1e-6) ||    // chronology does not matter
            (chrono == 1 && t2 - t1 > 1e-6)           ||    // chronological order
            (chrono == 2 && t1 - t2 > 1e-6))          &&    // antichronological order
           std::abs(t1 - t2) < this->SensorTimeThreshold; // Time duration is not too long
  };

  auto prevNeighIt = queryIt;
  auto nextNeighIt = queryIt;

  // Get in which trajectory section the query is
  // to only extract poses from the same section
  int querySection = this->GetTrajSection(queryIt->Time);

  int chrono = 0;
  // Increase the window until the number of data is reached
  while (std::distance(prevNeighIt, nextNeighIt) < windowWidth - 1)
  {
    // Check the next oldest neighbor
    bool prevIsValid = prevNeighIt != this->LogStates.begin() &&
                       querySection == this->GetTrajSection(std::prev(prevNeighIt)->Time) &&
                       checkTimes(std::prev(prevNeighIt)->Time, prevNeighIt->Time, chrono);
    // Check the next newest neighbor
    bool nextIsValid = std::next(nextNeighIt) != this->LogStates.end() &&
                       querySection == this->GetTrajSection(std::next(nextNeighIt)->Time) &&
                       checkTimes(std::next(nextNeighIt)->Time, nextNeighIt->Time, chrono);

    // Break if none is valid
    if (!prevIsValid && !nextIsValid)
      break;

    // Select the best valid one
    if (prevIsValid &&
        (!nextIsValid || std::abs(std::next(nextNeighIt)->Time - queryIt->Time) >
                         std::abs(std::prev(prevNeighIt)->Time - queryIt->Time)))
    {
      // The oldest is the best
      --prevNeighIt;
      // Set chronology for next added elements
      chrono = queryIt->Time > prevNeighIt->Time ? 1 : 2;
    }
    else
    {
      // The newest is the best
      ++nextNeighIt;
      // Set chronology for next added elements
      chrono = queryIt->Time < nextNeighIt->Time ? 1 : 2;
    }
  }
  ++nextNeighIt;

  std::vector<PoseStamped> statesWindow;
  statesWindow.reserve(std::distance(prevNeighIt, nextNeighIt));
  for (auto it = prevNeighIt; it != nextNeighIt; ++it)
    statesWindow.emplace_back(it->Isometry, it->Time);

  return statesWindow;
}

//-----------------------------------------------------------------------------
void Slam::UndistortWithLogStates(PointCloud::Ptr pcIn, PointCloud::Ptr pcOut,
                                  bool addTworld,
                                  int startIdx, int endIdx,
                                  Eigen::Isometry3d baseToPointsRef,
                                  double timeOffset) const
{
  if ((!addTworld && this->LogStates.size() < 2) ||
      (addTworld && this->LogStates.empty()))
  {
    PRINT_WARNING("Not enough states logged, cannot undistort")
    return;
  }

  // Build vector to compute interpolation model with previous logstates
  auto lastStateIt = std::prev(this->LogStates.end());

  unsigned int nbRequiredData = Interpolation::ModelRequiredNbData.at(this->Interpolation);
  if (addTworld)
    --nbRequiredData; // Tworld will be added
  std::vector<PoseStamped> ctrlPoses = this->GetStatesWindow(lastStateIt, nbRequiredData);

  Eigen::Isometry3d refFrameInv;
  double refTime;

  // If addTworld, Tworld is also used to undistort
  if (addTworld)
  {
    // Check if current time is consistent chronologically with previous times
    if (ctrlPoses.size() > 1 &&
        (ctrlPoses.back().Time - std::prev(std::prev(ctrlPoses.end()))->Time) *
        (this->CurrentTime - ctrlPoses.back().Time) < 0)
      // If times are not consistent, just keep last pose
      ctrlPoses = {ctrlPoses.back()};

    // Check if Tworld and the ctrlPoses are in the same trajectory section
    if (this->GetTrajSection(ctrlPoses.back().Time) != this->GetTrajSection(this->CurrentTime))
    {
      PRINT_WARNING("SLAM has not fully recovered, cannot undistort")
      return;
    }

    // Add Tworld to control poses
    ctrlPoses.emplace_back(this->Tworld, this->CurrentTime);
    // Update reference
    refFrameInv = this->Tworld.inverse();
    refTime = this->CurrentTime;
  }
  else
  {
    refFrameInv = lastStateIt->Isometry.inverse();
    refTime = lastStateIt->Time;
  }

  // Check interpolation data
  if (ctrlPoses.size() < 2)
  {
    PRINT_WARNING("Not enough usable logged states, cannot undistort")
    return;
  }

  // Put the logStates into the ref frame
  for (auto& p : ctrlPoses)
    p.Pose = refFrameInv * p.Pose;

  Interpolation::Trajectory motionInterpo(this->Interpolation, ctrlPoses);

  // If interpolation is unusable, don't undistorted
  if (!motionInterpo.CanInterpolate())
  {
    PRINT_WARNING("Cannot perform undistortion with logged states");
    return ;
  }

  if (endIdx < 0)
    endIdx = pcIn->size();

  // Undistort
  #pragma omp parallel for num_threads(this->NbThreads)
  for (int i = startIdx; i < endIdx; ++i)
    Utils::TransformPoint(pcIn->at(i), pcOut->at(i), motionInterpo(refTime + pcIn->at(i).time + timeOffset) * baseToPointsRef);

  PRINT_VERBOSE(3, "Undistortion performed using SLAM poses interpolation")
}

//-----------------------------------------------------------------------------
void Slam::UndistortWithPoseMeasurement(PointCloud::Ptr pc, double refTime,
                                        int startIdx, int endIdx,
                                        Eigen::Isometry3d baseToPointsRef,
                                        double timeOffset) const
{
  if (!this->PoseHasData())
  {
    PRINT_WARNING("External poses are empty : cannot use them to undistort input pointcloud")
    return;
  }

  ExternalSensors::PoseMeasurement synchPoseMeasCurrent; // Virtual measure with synchronized timestamp and calibration applied

  if (!this->PoseManager->ComputeSynchronizedMeasureBase(refTime, synchPoseMeasCurrent))
  {
    PRINT_WARNING("Could not perform undistortion using external poses supplied")
    return;
  }

  if (endIdx < 0)
    endIdx = pc->size();

  // Compute synchronized measures (not parallelizable)
  std::vector<ExternalSensors::PoseMeasurement> synchMeas(pc->size()); // Virtual measures with synchronized timestamp and calibration applied

  // Compute the synchronized pose for each point
  for (int idxPt = startIdx; idxPt < endIdx; ++idxPt)
    this->PoseManager->ComputeSynchronizedMeasureBase(refTime + pc->at(idxPt).time + timeOffset, synchMeas[idxPt]);

  // Compute synchronized poses for each point
  Eigen::Isometry3d invSynchPoseMeasCurrent = synchPoseMeasCurrent.Pose.inverse();

  // Transform with computed measures (parallelized)
  #pragma omp parallel for num_threads(this->NbThreads)
  for (int idxPt = startIdx; idxPt < endIdx; ++idxPt)
  {
    // Get transform from base at current time to base at point time
    Eigen::Isometry3d update = invSynchPoseMeasCurrent * synchMeas[idxPt].Pose * baseToPointsRef;
    Utils::TransformPoint(pc->at(idxPt), update);
  }

  PRINT_VERBOSE(3, "Undistortion performed using external poses interpolation")
}

//==============================================================================
//   Confidence estimators
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::SetOverlapSamplingRatio (float _arg)
{
  // Clamp ratio beteen 0 and 1
  this->OverlapSamplingRatio = Utils::Clamp(_arg, 0.f, 1.f);

  // Reset overlap value if overlap computation is disabled
  if (this->OverlapSamplingRatio == 0.)
    this->OverlapEstimation = -1.f;
}

//-----------------------------------------------------------------------------
void Slam::EstimateOverlap()
{
  if (this->OverlapSamplingRatio <= 0.f || !this->OptimizationValid)
  {
    this->OverlapEstimation = 1.f;
    return;
  }

  // Aggregate all input points into WORLD coordinates
  PointCloud::Ptr aggregatedPoints = this->GetRegisteredFrame();

  // Keep only the maps to use
  Maps mapsToUse;
  for (auto k : this->UsableKeypoints)
  {
    if (this->LocalMaps[k]->IsSubMapKdTreeValid())
      mapsToUse[k] = this->LocalMaps[k];
  }

  // Compute LCP like estimator
  // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info)
  this->OverlapEstimation = Confidence::LCPEstimator(aggregatedPoints, mapsToUse, this->OverlapSamplingRatio, this->NbThreads);
  PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : "
                                << static_cast<int>(aggregatedPoints->size() * this->OverlapSamplingRatio) << " points.");
}

//-----------------------------------------------------------------------------
float Slam::GetPositionErrorStd() const
{
  if (!this->OptimizationValid)
    return 0.f;
  return this->LocalizationUncertainty.PositionError;
}

//-----------------------------------------------------------------------------
void Slam::CheckMotionLimits()
{
  if (this->GetConfidenceWindow() <= 0 || !this->OptimizationValid)
  {
    this->ComplyMotionLimits = true;
    return;
  }

  this->MotionCheck.SetNewPose(this->Tworld, this->CurrentTime);
  this->ComplyMotionLimits = this->MotionCheck.isMotionValid();

  if (!this->ComplyMotionLimits)
    PRINT_WARNING("Motion passed limits : SLAM may have failed")
}

//==============================================================================
//   Transformation helpers
//==============================================================================

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::TransformPointCloud(PointCloud::ConstPtr cloud,
                                                const Eigen::Isometry3d& tf,
                                                const std::string& frameId) const
{
  // Copy all fields and set frame ID
  PointCloud::Ptr transformedCloud(new PointCloud);
  pcl::copyPointCloud(*cloud, *transformedCloud);
  transformedCloud->header.frame_id = frameId;

  // Transform each point inplace in parallel
  int nbPoints = transformedCloud->size();
  #pragma omp parallel for num_threads(this->NbThreads)
  for (int i = 0; i < nbPoints; ++i)
  {
    auto& point = transformedCloud->at(i);
    Utils::TransformPoint(point, tf);
  }
  return transformedCloud;
}

//-----------------------------------------------------------------------------
Slam::PointCloud::Ptr Slam::AggregateFrames(const std::vector<PointCloud::Ptr>& frames, bool worldCoordinates, bool undistort) const
{
  PointCloud::Ptr aggregatedFrames(new PointCloud);
  aggregatedFrames->header = Utils::BuildPclHeader(this->CurrentFrames[0]->header.stamp,
                                                   worldCoordinates ? this->WorldFrameId : this->BaseFrameId,
                                                   this->NbrFrameProcessed);

  // Loop over frames of input
  for (const auto& frame: frames)
  {
    // If the frame is empty, ignore it
    if (frame->empty())
      continue;

    // Add frame to aggregated output
    int startIdx = aggregatedFrames->size();
    int endIdx = startIdx + frame->size();
    *aggregatedFrames += *frame;

    // Modify point-wise time offsets to match header.stamp
    // And transform points from LIDAR to BASE or WORLD coordinate system
    double timeOffset = Utils::PclStampToSec(frame->header.stamp) - Utils::PclStampToSec(aggregatedFrames->header.stamp);
    Eigen::Isometry3d baseToLidar = this->GetBaseToLidarOffset(frame->front().device_id);

    // Undistort to represent all points from startIdx in base at current time
    // Rigid transform from LIDAR to BASE then undistortion from BASE to WORLD
    if (undistort)
    {
      if (this->Undistortion != UndistortionMode::EXTERNAL || !this->PoseHasData())
      {
        // Undistort using interpolation between logged states
        this->UndistortWithLogStates(aggregatedFrames, aggregatedFrames,
                                     false, // false -> Tworld is not used for the undistortion
                                     startIdx, aggregatedFrames->size(),
                                     baseToLidar, timeOffset);
      }
      else
      {
        // Undistort using interpolation between logged measurements
        this->UndistortWithPoseMeasurement(aggregatedFrames, this->CurrentTime,
                                           startIdx, aggregatedFrames->size(),
                                           baseToLidar, timeOffset);
      }

      // Update times
      #pragma omp parallel for num_threads(this->NbThreads)
      for (int idxPt = startIdx; idxPt < int(aggregatedFrames->size()); ++idxPt)
      {
        aggregatedFrames->at(idxPt).time += timeOffset;
        // Transform point to world frame if requested
        if (worldCoordinates)
          Utils::TransformPoint(aggregatedFrames->at(idxPt), this->Tworld);
      }
    }

    // Rigid transform from LIDAR to BASE or WORLD coordinate system
    else
    {
      // Get rigid transform to apply
      Eigen::Isometry3d tf = worldCoordinates ? this->Tworld * baseToLidar : baseToLidar;
      // If transform to apply is identity, avoid much work
      if (tf.isApprox(Eigen::Isometry3d::Identity()))
      {
        #pragma omp parallel for num_threads(this->NbThreads)
        for (int i = startIdx; i < endIdx; ++i)
          aggregatedFrames->at(i).time += timeOffset;
      }
      // If transform is non trivial, run transformation
      else
      {
        #pragma omp parallel for num_threads(this->NbThreads)
        for (int i = startIdx; i < endIdx; ++i)
        {
          aggregatedFrames->at(i).time += timeOffset;
          Utils::TransformPoint(aggregatedFrames->at(i), tf);
        }
      }
    }
  }

  return aggregatedFrames;
}

//==============================================================================
//   External sensors
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::InitWheelOdom()
{
  this->WheelOdomManager = std::make_shared<ExternalSensors::WheelOdometryManager>(0.,
                                                                                   this->SensorTimeOffset,
                                                                                   this->SensorTimeThreshold,
                                                                                   this->SensorMaxMeasures,
                                                                                   this->Verbosity >= 3);
}

//-----------------------------------------------------------------------------
void Slam::InitGravity()
{
  this->GravityManager = std::make_shared<ExternalSensors::ImuGravityManager>(0.,
                                                                              this->SensorTimeOffset,
                                                                              this->SensorTimeThreshold,
                                                                              this->SensorMaxMeasures,
                                                                              this->Verbosity >= 3);
}

//-----------------------------------------------------------------------------
void Slam::InitImu()
{
  this->ImuManager = std::make_shared<ExternalSensors::ImuManager>(0.,
                                                                   this->SensorTimeOffset,
                                                                   this->SensorTimeThreshold,
                                                                   this->SensorMaxMeasures,
                                                                   this->Interpolation,
                                                                   this->Tworld,
                                                                   this->Verbosity >= 3);
}

//-----------------------------------------------------------------------------
void Slam::InitLandmarkManager(int id)
{
  this->LandmarksManagers[id] = ExternalSensors::LandmarkManager(this->LandmarkWeight,
                                                                 this->SensorTimeOffset,
                                                                 this->SensorTimeThreshold,
                                                                 this->SensorMaxMeasures,
                                                                 this->Interpolation,
                                                                 this->LandmarkPositionOnly,
                                                                 this->Verbosity >= 3);

  this->LandmarksManagers[id].SetSaturationDistance(this->LandmarkSaturationDistance);
  // The calibration can be modified afterwards
  this->LandmarksManagers[id].SetCalibration(this->LmDetectorCalibration);
}

//-----------------------------------------------------------------------------
void Slam::InitGps()
{
  this->GpsManager = std::make_shared<ExternalSensors::GpsManager>(this->SensorTimeOffset,
                                                                   this->SensorTimeThreshold,
                                                                   this->SensorMaxMeasures,
                                                                   this->Verbosity >= 3);
}

//-----------------------------------------------------------------------------
void Slam::InitPoseSensor()
{
  this->PoseManager = std::make_shared<ExternalSensors::PoseManager>(this->PoseWeight,
                                                                     this->SensorTimeOffset,
                                                                     this->SensorTimeThreshold,
                                                                     this->SensorMaxMeasures,
                                                                     this->Interpolation,
                                                                     this->Verbosity >= 3);
}

//-----------------------------------------------------------------------------
void Slam::InitCamera()
{
  this->CameraManager = std::make_shared<ExternalSensors::CameraManager>(0.,
                                                                         this->SensorTimeOffset,
                                                                         this->SensorTimeThreshold,
                                                                         this->SensorMaxMeasures,
                                                                         this->Verbosity >= 3);
}

// Sensor data
//-----------------------------------------------------------------------------

// Wheel odometer
//-----------------------------------------------------------------------------
void Slam::AddWheelOdomMeasurement(const ExternalSensors::WheelOdomMeasurement& om)
{
  if (!this->WheelOdomManager)
    this->InitWheelOdom();
  this->WheelOdomManager->AddMeasurement(om);
}

// IMU gravity
//-----------------------------------------------------------------------------
void Slam::AddGravityMeasurement(const ExternalSensors::GravityMeasurement& gm)
{
  if (!this->GravityManager)
    this->InitGravity();
  this->GravityManager->AddMeasurement(gm);
}

//-----------------------------------------------------------------------------
void Slam::AddImuMeasurement(const ExternalSensors::ImuMeasurement& m)
{
  if (!this->ImuManager)
    this->InitImu();
  this->ImuManager->AddMeasurement(m);
  if (!this->PoseHasData())
    this->PoseManager = this->ImuManager;
}

//-----------------------------------------------------------------------------
void Slam::SetImuCalibration(const Eigen::Isometry3d& calib)
{
  if (!this->ImuManager)
    this->InitImu();
  this->ImuManager->SetCalibration(calib);
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetTworld(double time, bool trackTime)
{
  if (this->LogStates.empty())
    return this->TworldInit;

  if (time < 0)
    return this->LogStates.back().Isometry;

  if (time < this->LogStates.front().Time)
  {
    PRINT_WARNING("The oldest stored state is newer than the requested time. The oldest state is returned. ")
    return this->LogStates.front().Isometry;
  }

  // If IMU data available ask for pose to the IMU manager which has its own pose graph
  if (this->ImuHasData())
    return this->ImuManager->GetPose(time);
  else if (this->PoseHasData())
  {
    // If ext pose data available, get the state bounds and interpolate using external poses
    std::vector<PoseStamped> bounds = this->GetStatesWindow(this->GetClosestState(time), 2);

    // If external poses are available, deduce the pose at the input time with external pose measurement
    ExternalSensors::PoseMeasurement synchMeasInf, synchMeas;
    if (this->PoseManager->ComputeSynchronizedMeasureBase(bounds.front().Time, synchMeasInf, trackTime) &&
        this->PoseManager->ComputeSynchronizedMeasureBase(time, synchMeas, trackTime))
    {
      Eigen::Isometry3d tRelative = synchMeasInf.Pose.inverse() * synchMeas.Pose;
      if (time > bounds.back().Time)
        PRINT_WARNING("Time " << time << " has not been reached yet and the pose is extrapolated")
      return bounds.front().Pose * tRelative;
    }
  }

  // If external data cannot be used, directly interpolate last states
  std::vector<PoseStamped> vecPose = this->GetStatesWindow(this->GetClosestState(time),
                                                           Interpolation::ModelRequiredNbData.at(this->Interpolation));

  return Interpolation::Interpolate(vecPose, time, this->Interpolation);
}

// Landmark detector
//-----------------------------------------------------------------------------
void Slam::AddLandmarkMeasurement(const ExternalSensors::LandmarkMeasurement& lm, int id)
{
  if (!this->LandmarksManagers.count(id))
    this->InitLandmarkManager(id);
  this->LandmarksManagers[id].AddMeasurement(lm);
}

//-----------------------------------------------------------------------------
void Slam::AddLandmarkManager(int id, const Eigen::Vector6d& absolutePose, const Eigen::Matrix6d& absolutePoseCovariance)
{
  if (!this->LandmarksManagers.count(id))
    this->InitLandmarkManager(id);
  this->LandmarksManagers[id].SetAbsolutePose(absolutePose, absolutePoseCovariance);
}

//-----------------------------------------------------------------------------
void Slam::SetLmDetectorCalibration(const Eigen::Isometry3d& calib)
{
  this->LmDetectorCalibration = calib;
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetCalibration(calib);
}

//-----------------------------------------------------------------------------
bool Slam::LmCanBeUsedLocally() const
{
  bool lmCanBeUsed = false;
  for (auto& idLm : this->LandmarksManagers)
  {
    if (idLm.second.CanBeUsedLocally())
    {
      lmCanBeUsed = true;
      break;
    }
  }
  return lmCanBeUsed;
}

//-----------------------------------------------------------------------------
bool Slam::LmHasData() const
{
  bool lmHasData = false;
  for (auto& idLm : this->LandmarksManagers)
  {
    if (idLm.second.HasData())
    {
      lmHasData = true;
      break;
    }
  }
  return lmHasData;
}

// GPS
//-----------------------------------------------------------------------------
void Slam::AddGpsMeasurement(const ExternalSensors::GpsMeasurement& gpsMeas)
{
  if (!this->GpsManager)
    this->InitGps();
  this->GpsManager->AddMeasurement(gpsMeas);
}

//-----------------------------------------------------------------------------
void Slam::SetGpsCalibration(const Eigen::Isometry3d& calib)
{
  if (!this->GpsManager)
    this->InitGps();
  this->GpsManager->SetCalibration(calib);
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetGpsCalibration()
{
  if (!this->GpsManager)
  {
    PRINT_ERROR("Cannot get GPS calibration : GPS not enabled")
    return Eigen::Isometry3d::Identity();
  }
  return this->GpsManager->GetCalibration();
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetGpsOffset()
{
  if (!this->GpsManager)
  {
    PRINT_ERROR("Cannot get GPS offset : GPS not enabled")
    return Eigen::Isometry3d::Identity();
  }
  return this->GpsManager->GetOffset();
}

//-----------------------------------------------------------------------------
bool Slam::CalibrateWithGps()
{
  if (!this->GpsHasData())
  {
    PRINT_ERROR("Cannot get GPS offset : GPS not enabled or GPS data not available")
    return false;
  }

  // The search for a synchronized data is one-time so we
  // don't want to keep track of time for next searches (see ComputeSynchronizedMeasure)
  bool trackTime = false;

  // Initialize GPS offset with first synchronized measurements
  // The first graph optimizations will mainly correct the orientations
  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
  for (auto& s : this->LogStates)
  {
    ExternalSensors::GpsMeasurement gpsSynchMeasure; // Virtual measure with synchronized timestamp and no offset applied
    if (this->GpsManager->ComputeSynchronizedMeasure(s.Time, gpsSynchMeasure, trackTime))
    {
      offset.translation() = (s.Isometry * this->GpsManager->GetCalibration()).translation() - gpsSynchMeasure.Position;
      break;
    }
  }
  this->GpsManager->SetOffset(offset);

  // Optimize the graph : add lidar states and link with fixed gps states
  if (!this->OptimizeGraph())
    return false;

  // Reset poses in odom frame (first Lidar pose)
  Eigen::Isometry3d firstInverse = this->LogStates.front().Isometry.inverse();
  for (auto& s : this->LogStates)
  {
    // Rotate covariance
    Eigen::Vector6d initPose = Utils::IsometryToXYZRPY(s.Isometry);
    CeresTools::RotateCovariance(initPose, s.Covariance, firstInverse, true); // new = first^-1 * init
    // Transform pose
    s.Isometry = firstInverse * s.Isometry;
  }

  // Update the maps and the pose with the new trajectory
  this->UpdateMaps();
  this->SetWorldTransformFromGuess(this->LogStates.back().Isometry);

  // Set refined offset : first Lidar pose + initial offset
  firstInverse.translation() += offset.translation();
  this->GpsManager->SetOffset(firstInverse);

  return true;
}

// Pose
//-----------------------------------------------------------------------------
void Slam::AddPoseMeasurement(const ExternalSensors::PoseMeasurement& pm)
{
  if (!this->PoseManager)
    this->InitPoseSensor();
  this->PoseManager->AddMeasurement(pm);
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetPoseCalibration() const
{
  if (!this->PoseManager)
    return Eigen::Isometry3d::Identity();
  return this->PoseManager->GetCalibration();
}

//-----------------------------------------------------------------------------
void Slam::SetPoseCalibration(const Eigen::Isometry3d& calib)
{
  if (!this->PoseManager)
    this->InitPoseSensor();
  this->PoseManager->SetCalibration(calib);
}

//-----------------------------------------------------------------------------
bool Slam::CalibrateWithExtPoses()
{
  if (!this->PoseManager)
    return false;
  return this->PoseManager->ComputeCalibration(this->LogStates);
}

// RGB camera
//-----------------------------------------------------------------------------
void Slam::AddCameraImage(const ExternalSensors::Image& image)
{
  if (!this->CameraManager)
    this->InitCamera();

  this->CameraManager->AddMeasurement(image);
}

// Sensors' parameters
//-----------------------------------------------------------------------------
void Slam::ResetSensors(bool emptyMeasurements)
{
  ExtSensorMacro(Reset(emptyMeasurements))
  this->ImuHasBeenUpdated = 0;
  if (emptyMeasurements && this->PoseManager)
    // Break link between IMU and Pose managers
    this->InitPoseSensor();
}

//-----------------------------------------------------------------------------
void Slam::ResetSensor(bool emptyMeasurements, ExternalSensor sensor)
{
  switch(sensor)
  {
    case ExternalSensor::WHEEL_ODOM:
    {
      if (this->WheelOdomManager)
        this->WheelOdomManager->Reset(emptyMeasurements);
      break;
    }
    case ExternalSensor::IMU:
    {
      if (this->ImuManager)
        this->ImuManager->Reset(emptyMeasurements);
      break;
    }
    case ExternalSensor::LANDMARK_DETECTOR:
    {
      for (auto& idLm : this->LandmarksManagers)
        idLm.second.Reset(emptyMeasurements);
      break;
    }
    case ExternalSensor::POSE:
    {
      if (this->PoseManager)
        this->PoseManager->Reset(emptyMeasurements);
      if (emptyMeasurements && this->PoseManager)
        // Break link between IMU and Pose managers
        this->InitPoseSensor();
      break;
    }
    case ExternalSensor::CAMERA:
    {
      if (this->CameraManager)
        this->CameraManager->Reset(emptyMeasurements);
      break;
    }
  }
}

//-----------------------------------------------------------------------------
void Slam::SetSensorTimeOffset(double timeOffset)
{
  ExtSensorMacro(SetTimeOffset(timeOffset))
  this->SensorTimeOffset = timeOffset;
}

//-----------------------------------------------------------------------------
void Slam::SetSensorTimeThreshold(double thresh)
{
  ExtSensorMacro(SetTimeThreshold(thresh))
  this->SensorTimeThreshold = thresh;
}

//-----------------------------------------------------------------------------
void Slam::SetSensorMaxMeasures(unsigned int max)
{
  ExtSensorMacro(SetMaxMeasures(max))
  this->SensorMaxMeasures = max;
}

// Wheel odometer
//-----------------------------------------------------------------------------
double Slam::GetWheelOdomWeight() const
{
  if(this->WheelOdomManager)
    return this->WheelOdomManager->GetWeight();
  PRINT_ERROR("Wheel odometer has not been set : can't get wheel odom weight")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetWheelOdomWeight(double weight)
{
  if(!this->WheelOdomManager)
    this->InitWheelOdom();
  this->WheelOdomManager->SetWeight(weight);
}

//-----------------------------------------------------------------------------
bool Slam::GetWheelOdomRelative() const
{
  if(this->WheelOdomManager)
    return this->WheelOdomManager->GetRelative();
  PRINT_ERROR("Wheel odometer has not been set : can't get wheel odom relative boolean")
  return false;
}

//-----------------------------------------------------------------------------
void Slam::SetWheelOdomRelative(bool isRelative)
{
  if(!this->WheelOdomManager)
    this->InitWheelOdom();
  this->WheelOdomManager->SetRelative(isRelative);
}

// IMU gravity
//-----------------------------------------------------------------------------
double Slam::GetGravityWeight() const
{
  if(this->GravityManager)
    return this->GravityManager->GetWeight();
  PRINT_ERROR("IMU has not been set : can't get IMU weight")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetGravityWeight(double weight)
{
  if(!this->GravityManager)
    this->InitGravity();
  this->GravityManager->SetWeight(weight);
}

//-----------------------------------------------------------------------------
double Slam::GetImuWeight() const
{
  if(this->ImuManager)
    return this->ImuManager->GetWeight();
  PRINT_ERROR("IMU has not been set : can't get IMU weight")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetImuWeight(double weight)
{
  if(!this->ImuManager)
    this->InitImu();
  this->ImuManager->SetWeight(weight);
}

//-----------------------------------------------------------------------------
Eigen::Vector3d Slam::GetImuGravity() const
{
  if(this->ImuManager)
    return this->ImuManager->GetGravity();
  PRINT_ERROR("IMU has not been set : can't get IMU gravity")
  return {0., 0., 0.};
}

//-----------------------------------------------------------------------------
void Slam::SetImuGravity(const Eigen::Vector3d& gravity)
{
  if(!this->ImuManager)
    this->InitImu();
  this->ImuManager->SetGravity(gravity);
}

//-----------------------------------------------------------------------------
float Slam::GetImuFrequency() const
{
  if(this->ImuManager)
    return this->ImuManager->GetFrequency();
  PRINT_ERROR("IMU has not been set : can't get IMU frequency")
  return 0;
}

//-----------------------------------------------------------------------------
void Slam::SetImuFrequency(float frequency)
{
  if(!this->ImuManager)
    this->InitImu();
  this->ImuManager->SetFrequency(frequency);
}

//-----------------------------------------------------------------------------
unsigned int Slam::GetImuResetThreshold() const
{
  if(this->ImuManager)
    return this->ImuManager->GetResetThreshold();
  PRINT_ERROR("IMU has not been set : can't get IMU reset threshold")
  return 0;
}

//-----------------------------------------------------------------------------
void Slam::SetImuResetThreshold(unsigned int thresh)
{
  if(!this->ImuManager)
    this->InitImu();
  this->ImuManager->SetResetThreshold(thresh);
}

// Landmark detector
//-----------------------------------------------------------------------------
void Slam::SetLandmarkWeight(double weight)
{
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetWeight(weight);
  this->LandmarkWeight = weight;
}

//-----------------------------------------------------------------------------
void Slam::SetLandmarkSaturationDistance(float dist)
{
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetSaturationDistance(dist);
  this->LandmarkSaturationDistance = dist;
}

//-----------------------------------------------------------------------------
void Slam::SetLandmarkPositionOnly(bool positionOnly)
{
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetPositionOnly(positionOnly);
  this->LandmarkPositionOnly = positionOnly;
}

//-----------------------------------------------------------------------------
void Slam::SetLandmarkCovarianceRotation(bool rotate)
{
  for (auto& idLm : this->LandmarksManagers)
    idLm.second.SetCovarianceRotation(rotate);
  this->LandmarkCovarianceRotation = rotate;
}

// Pose
//-----------------------------------------------------------------------------
double Slam::GetPoseWeight() const
{
  if (this->PoseManager)
    return this->PoseManager->GetWeight();
  PRINT_ERROR("Pose sensor has not been set : can't get pose weight")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetPoseWeight(double weight)
{
  if (!this->PoseManager)
    this->InitPoseSensor();
  this->PoseWeight = weight;
  this->PoseManager->SetWeight(weight);
}

// RGB camera
//-----------------------------------------------------------------------------
double Slam::GetCameraWeight() const
{
  if (this->CameraManager)
    return this->CameraManager->GetWeight();
  PRINT_ERROR("Camera has not been set : can't get camera weight")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetCameraWeight(double weight)
{
  if (!this->CameraManager)
    this->InitCamera();
  this->CameraManager->SetWeight(weight);
}

//-----------------------------------------------------------------------------
void Slam::SetCameraCalibration(const Eigen::Isometry3d& calib)
{
  if (!this->CameraManager)
    this->InitCamera();
  this->CameraManager->SetCalibration(calib);
}

//-----------------------------------------------------------------------------
void Slam::SetCameraIntrinsicCalibration(const Eigen::Matrix3f& k)
{
  if (!this->CameraManager)
    this->InitCamera();
  this->CameraManager->SetIntrinsicCalibration(k);
}

//-----------------------------------------------------------------------------
float Slam::GetCameraSaturationDistance() const
{
  if (this->CameraManager)
    return this->CameraManager->GetSaturationDistance();
  PRINT_ERROR("Camera has not been set : can't get camera saturation distance")
  return 0.;
}

//-----------------------------------------------------------------------------
void Slam::SetCameraSaturationDistance(float dist)
{
  if (!this->CameraManager)
    this->InitCamera();
  this->CameraManager->SetSaturationDistance(dist);
}

//==============================================================================
//   Keypoints extraction parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
std::map<uint8_t, Slam::KeypointExtractorPtr> Slam::GetKeyPointsExtractors() const
{
  return this->KeyPointsExtractors;
}
void Slam::SetKeyPointsExtractors(const std::map<uint8_t, KeypointExtractorPtr>& extractors)
{
  this->KeyPointsExtractors = extractors;
}

//-----------------------------------------------------------------------------
Slam::KeypointExtractorPtr Slam::GetKeyPointsExtractor(uint8_t deviceId) const
{
  return this->KeyPointsExtractors.count(deviceId) ? this->KeyPointsExtractors.at(deviceId) : KeypointExtractorPtr();
}
void Slam::SetKeyPointsExtractor(KeypointExtractorPtr extractor, uint8_t deviceId)
{
  this->KeyPointsExtractors[deviceId] = extractor;
}

//-----------------------------------------------------------------------------
Eigen::Isometry3d Slam::GetBaseToLidarOffset(uint8_t deviceId) const
{
  return this->BaseToLidarOffsets.count(deviceId) ? this->BaseToLidarOffsets.at(deviceId) : Eigen::UnalignedIsometry3d::Identity();
}
void Slam::SetBaseToLidarOffset(const Eigen::Isometry3d& transform, uint8_t deviceId)
{
  this->BaseToLidarOffsets[deviceId] = transform;
}

//==============================================================================
//   Rolling grids parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::ClearMaps(Maps& maps)
{
  for (auto kmap: maps)
    kmap.second->Reset();
}

//-----------------------------------------------------------------------------
void Slam::ClearLog()
{
  auto prevlastLogStateIt = LogStates.end();
  --prevlastLogStateIt;
  --prevlastLogStateIt;
  std::list<LidarState> storeLog;
  storeLog.push_back(*prevlastLogStateIt);
  storeLog.push_back(*(++prevlastLogStateIt));

  this->LogStates.clear();
  this->LogStates = storeLog;
}

//-----------------------------------------------------------------------------
double Slam::GetVoxelGridDecayingThreshold() const
{
  return this->LocalMaps.begin()->second->GetDecayingThreshold();
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridDecayingThreshold(double decay)
{
  for (auto k : this->UsableKeypoints)
    this->LocalMaps[k]->SetDecayingThreshold(decay);
}

//-----------------------------------------------------------------------------
SamplingMode Slam::GetVoxelGridSamplingMode(Keypoint k) const
{
  return this->LocalMaps.at(k)->GetSampling();
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridSamplingMode(Keypoint k, SamplingMode sm)
{
  this->LocalMaps[k]->SetSampling(sm);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridLeafSize(Keypoint k, double size)
{
  this->LocalMaps[k]->SetLeafSize(size);
}

//-----------------------------------------------------------------------------
double Slam::GetVoxelGridLeafSize(Keypoint k) const
{
  return this->LocalMaps.at(k)->GetLeafSize();
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridSize(int size)
{
  for (auto k : this->UsableKeypoints)
    this->LocalMaps[k]->SetGridSize(size);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridResolution(double resolution)
{
  for (auto k : this->UsableKeypoints)
    this->LocalMaps[k]->SetVoxelResolution(resolution);
}

//-----------------------------------------------------------------------------
void Slam::SetVoxelGridMinFramesPerVoxel(unsigned int minFrames)
{
  for (auto k : this->UsableKeypoints)
    this->LocalMaps[k]->SetMinFramesPerVoxel(minFrames);
}

//==============================================================================
//   Memory parameters setting
//==============================================================================

//-----------------------------------------------------------------------------
void Slam::SetLoggingTimeout(double lMax)
{
  this->LoggingTimeout = lMax;
  double currentTime = this->LogStates.back().Time;
  auto itSt = this->LogStates.begin();
  while (currentTime - itSt->Time > lMax &&
         this->LogStates.size() > Interpolation::ModelRequiredNbData.at(this->Interpolation))
  {
    ++itSt;
    this->LogStates.pop_front();
  }
}

} // end of LidarSlam namespace